src/sensor/rotation_vector/design/lib/estimate_orientation.m

   1 % estimate_orientation
   2 %
   3 % Copyright (c) 2015 Samsung Electronics Co., Ltd.
   4 %
   5 % Licensed under the Apache License, Version 2.0 (the "License");
   6 % you may not use this file except in compliance with the License.
   7 % You may obtain a copy of the License at
   8 %
   9 % http://www.apache.org/licenses/LICENSE-2.0
  10 %
  11 % Unless required by applicable law or agreed to in writing, software
  12 % distributed under the License is distributed on an "AS IS" BASIS,
  13 % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14 % See the License for the specific language governing permissions and
  15 % limitations under the License.
  16
  17 % Orientation Estimation function
  18 %
  19 % - Orientation and rotation vector Estimation using Gyroscope as the driving system and
  20 %   Accelerometer+Geo-Magnetic Sensors as Aiding System.
  21 % - Gaming rotation vector Estimation using Accelerometer and Gyroscope sensors.
  22 % - Geomagnetic rotation vector Estimation using Accelerometer and Geomagnetic sensors.
  23 % - Quaternion based approach
  24 % - Estimation and correction of orientation errors and bias errors for gyroscope using Kalman filter
  25
  26 function [quat_driv, quat_aid, quat_error, gyro_bias]  = estimate_orientation(Accel_data, Gyro_data, Mag_data)
  27         PLOT_SCALED_SENSOR_COMPARISON_DATA = 1;
  28         PLOT_INDIVIDUAL_SENSOR_INPUT_DATA = 1;
  29
  30         MAGNETIC_ALIGNMENT_FACTOR = -1;
  31         GYRO_DATA_DISABLED = 0;
  32         MAG_DATA_DISABLED = 0;
  33
  34         if Gyro_data(4,1) == 0
  35                 GYRO_DATA_DISABLED = 1;
  36         end
  37
  38         if Mag_data(4,1) == 0
  39                 MAG_DATA_DISABLED = 1;
  40         end
  41
  42         GRAVITY = 9.80665;
  43         PI = 3.141593;
  44         ACCEL_THRESHOLD = 0.2;
  45         GYRO_THRESHOLD = 0.01;
  46         NON_ZERO_VAL = 0.1;
  47         PI_DEG = 180;
  48
  49         MOVING_AVERAGE_WINDOW_LENGTH = 20;
  50         RAD2DEG = 57.2957795;
  51         DEG2RAD = 0.0174532925;
  52         US2S =  1.0 / 1000000.0;
  53
  54         ZigmaW = 0.05 * DEG2RAD;
  55         TauW = 1000;
  56
  57         BUFFER_SIZE = size(Accel_data,2);
  58         Ax = Accel_data(1,:);
  59         Ay = Accel_data(2,:);
  60         Az = Accel_data(3,:);
  61         ATime = Accel_data(4,:);
  62
  63         mag_x = zeros(1,BUFFER_SIZE);
  64         mag_y = zeros(1,BUFFER_SIZE);
  65         mag_z = zeros(1,BUFFER_SIZE);
  66         MTime = zeros(1,BUFFER_SIZE);
  67
  68         gyro_bias = zeros(3,BUFFER_SIZE);
  69
  70         if MAG_DATA_DISABLED != 1
  71                 Mx = Mag_data(1,:);
  72                 My = Mag_data(2,:);
  73                 Mz = Mag_data(3,:);
  74                 MTime = Mag_data(4,:);
  75         end
  76
  77         acc_x = zeros(1,BUFFER_SIZE);
  78         acc_y = zeros(1,BUFFER_SIZE);
  79         acc_z = zeros(1,BUFFER_SIZE);
  80
  81         quat_aid = zeros(BUFFER_SIZE,4);
  82         quat_driv = zeros(BUFFER_SIZE,4);
  83         quat_gaming_rv = zeros(BUFFER_SIZE,4);
  84         quat_error = zeros(BUFFER_SIZE,4);
  85
  86         acc_e = [0.0;0.0;1.0]; % gravity vector in earth frame
  87         mag_e = [0.0;MAGNETIC_ALIGNMENT_FACTOR;0.0]; % magnetic field vector in earth frame
  88
  89         if GYRO_DATA_DISABLED != 1
  90                 % Gyroscope Bias Variables
  91                 Bx = 0; By = 0; Bz = 0;
  92
  93                 Gx = Gyro_data(1,:);
  94                 Gy = Gyro_data(2,:);
  95                 Gz = Gyro_data(3,:);
  96                 GTime = Gyro_data(4,:);
  97
  98                 gyr_x = zeros(1,BUFFER_SIZE);
  99                 gyr_y = zeros(1,BUFFER_SIZE);
 100                 gyr_z = zeros(1,BUFFER_SIZE);
 101
 102                 % User Acceleration mean and Variance
 103                 A_T = zeros(1,BUFFER_SIZE);
 104                 G_T = zeros(1,BUFFER_SIZE);
 105                 M_T = zeros(1,BUFFER_SIZE);
 106                 var_roll = zeros(1,BUFFER_SIZE);
 107                 var_pitch = zeros(1,BUFFER_SIZE);
 108                 var_yaw = zeros(1,BUFFER_SIZE);
 109                 var_Gx = zeros(1,BUFFER_SIZE);
 110                 var_Gy = zeros(1,BUFFER_SIZE);
 111                 var_Gz = zeros(1,BUFFER_SIZE);
 112
 113                 roll = zeros(1,BUFFER_SIZE);
 114                 pitch = zeros(1,BUFFER_SIZE);
 115                 yaw = zeros(1,BUFFER_SIZE);
 116
 117                 % system covariance matrix
 118                 Q = zeros(6,6);
 119
 120                 % measurement covariance matrix
 121                 R = zeros(6,6);
 122
 123                 A_T(1) = 100000;
 124                 G_T(1) = 100000;
 125                 M_T(1) = 100000;
 126
 127                 H = [eye(3) zeros(3,3); zeros(3,6)];
 128                 x = zeros(6,BUFFER_SIZE);
 129                 e = zeros(1,6);
 130                 P = 1 * eye(6);% state covariance matrix
 131
 132                 quat_driv(1,:) = [1 0 0 0];
 133         end
 134
 135         q = [1 0 0 0];
 136
 137         % first order filtering
 138         for i = 1:BUFFER_SIZE
 139                 % normalize accelerometer measurements
 140                 norm_acc = 1/sqrt(Ax(i)^2 + Ay(i)^2 + Az(i)^2);
 141                 acc_x(i) = norm_acc * Ax(i);
 142                 acc_y(i) = norm_acc * Ay(i);
 143                 acc_z(i) = norm_acc * Az(i);
 144
 145                 % gravity vector in body frame
 146                 acc_b =[acc_x(i);acc_y(i);acc_z(i)];
 147
 148                 if MAG_DATA_DISABLED != 1
 149                         % normalize magnetometer measurements
 150                         norm_mag = 1/sqrt(Mx(i)^2 + My(i)^2 + Mz(i)^2);
 151                         mag_x(i) = norm_mag * Mx(i);
 152                         mag_y(i) = norm_mag * My(i);
 153                         mag_z(i) = norm_mag * Mz(i);
 154
 155                         % Aiding System (Accelerometer + Geomagnetic) quaternion generation
 156                         % magnetic field vector in body frame
 157                         mag_b =[mag_x(i);mag_y(i);mag_z(i)];
 158
 159                         % compute measurement quaternion with TRIAD algorithm
 160                         acc_b_x_mag_b = cross(acc_b,mag_b);
 161                         acc_e_x_mag_e = cross(acc_e,mag_e);
 162                         M_b = [acc_b acc_b_x_mag_b cross(acc_b_x_mag_b,acc_b)];
 163                         M_e = [acc_e acc_e_x_mag_e cross(acc_e_x_mag_e,acc_e)];
 164                         Rot_m = M_e * M_b';
 165                         quat_aid(i,:) = rot_mat2quat(Rot_m);
 166                 else
 167                         axis = cross(acc_b, acc_e);
 168                         angle = acos(dot(acc_b, acc_e));
 169                         quat_aid(i,:) = axis_rot2quat(axis, angle);
 170                 end
 171
 172                 if GYRO_DATA_DISABLED != 1
 173                         gyr_x(i) = Gx(i) * PI;
 174                         gyr_y(i) = Gy(i) * PI;
 175                         gyr_z(i) = Gz(i) * PI;
 176
 177                         euler = quat2euler(quat_aid(i,:));
 178                         roll(i) = euler(2);
 179                         pitch(i) = euler(1);
 180                         yaw(i) = euler(3);
 181
 182                         if i > 1
 183                                 A_T(i) = ATime(i) - ATime(i-1);
 184                                 G_T(i) = GTime(i) - GTime(i-1);
 185                                 M_T(i) = MTime(i) - MTime(i-1);
 186                         end
 187
 188                         dt = G_T(i) * US2S;
 189
 190                         % Driving System (Gyroscope) quaternion generation
 191                         % convert scaled gyro data to rad/s
 192                         qDot = 0.5 * quat_prod(q, [0 gyr_x(i) gyr_y(i) gyr_z(i)]);
 193
 194                         % Integrate to yield quaternion
 195                         q = q + qDot * dt * PI;
 196
 197                         % normalise quaternion
 198                         quat_driv(i,:) = q / norm(q);
 199
 200                         % Kalman Filtering
 201                         quat_error(i,:) = quat_prod(quat_aid(i,:), quat_driv(i,:));
 202
 203                         euler_e = quat2euler(quat_error(i,:));
 204
 205                         gyr_x(i) = gyr_x(i) - (Bx + euler_e(1));
 206                         gyr_y(i) = gyr_y(i) - (By + euler_e(2));
 207                         gyr_z(i) = gyr_z(i) - (Bz + euler_e(3));
 208
 209                         if i <= MOVING_AVERAGE_WINDOW_LENGTH
 210                                 var_Gx(i) = NON_ZERO_VAL;
 211                                 var_Gy(i) = NON_ZERO_VAL;
 212                                 var_Gz(i) = NON_ZERO_VAL;
 213                                 var_roll(i) = NON_ZERO_VAL;
 214                                 var_pitch(i) = NON_ZERO_VAL;
 215                                 var_yaw(i) = NON_ZERO_VAL;
 216                         else
 217                                 var_Gx(i) = var(gyr_x((i-MOVING_AVERAGE_WINDOW_LENGTH):i));
 218                                 var_Gy(i) = var(gyr_y((i-MOVING_AVERAGE_WINDOW_LENGTH):i));
 219                                 var_Gz(i) = var(gyr_z((i-MOVING_AVERAGE_WINDOW_LENGTH):i));
 220                                 var_roll(i) = var(roll((i-MOVING_AVERAGE_WINDOW_LENGTH):i));
 221                                 var_pitch(i) = var(pitch((i-MOVING_AVERAGE_WINDOW_LENGTH):i));
 222                                 var_yaw(i) = var(yaw((i-MOVING_AVERAGE_WINDOW_LENGTH):i));
 223                         end
 224
 225                         Qwn = [var_Gx(i) 0 0;0 var_Gy(i) 0;0 0 var_Gz(i);];
 226                         Qwb = (2 * (ZigmaW^2) / TauW) * eye(3);
 227
 228                         % Process Noise Covariance
 229                         Q = [Qwn zeros(3,3);zeros(3,3) Qwb];
 230
 231                         % Measurement Noise Covariance
 232                         R = [[var_roll(i) 0 0;0 var_pitch(i) 0;0 0 var_yaw(i)] zeros(3,3); zeros(3,3) zeros(3,3)];
 233
 234                         % initialization for q
 235                         if i == 1
 236                                 q = quat_aid(i,:);
 237                         end
 238
 239                         F = [[0 gyr_z(i) -gyr_y(i);-gyr_z(i) 0 gyr_x(i);gyr_y(i) -gyr_x(i) 0] zeros(3,3); zeros(3,3) (-(1/TauW) * eye(3))];
 240
 241                         % Time Update
 242                         if i > 1
 243                                 x(:,i) = F * x(:,i-1);
 244                         end
 245
 246                         % compute covariance of prediction
 247                         P = (F * P * F') + Q;
 248
 249                         x1 = euler_e(1)'/PI;
 250                         x2 = euler_e(2)'/PI;
 251                         x3 = euler_e(3)'/PI;
 252
 253                         if MAG_DATA_DISABLED != 1
 254                                 q = quat_prod(quat_driv(i,:), [1 x1 x2 x3]) * PI;
 255                                 q = q / norm(q);
 256                         else
 257                                 euler_aid = quat2euler(quat_aid(i,:));
 258                                 euler_driv = quat2euler(quat_driv(i,:));
 259                                 if (acc_y(i)^2 < (ACCEL_THRESHOLD) && Gx(i)^2 < (GYRO_THRESHOLD))
 260                                         if (acc_x(i)^2 < (ACCEL_THRESHOLD) && Gy(i)^2 < (GYRO_THRESHOLD))
 261                                                 if (Gz(i)^2 < (GYRO_THRESHOLD))
 262                                                         q = euler2quat([euler_aid(2) euler_aid(1) euler_driv(3)]);
 263                                                 end
 264                                         end
 265                                 end
 266                         end
 267
 268                         if i > 1
 269                                 e = [x1 x2 x3 x(4,i) x(5,i) x(6,i)];
 270                         end
 271
 272                         for j =1:6
 273                                 % compute Kalman gain
 274                                 K(:,j) = P(j ,:)./(P(j,j)+R(j,j));
 275                                 % update state vector
 276                                 x(:,i) = x(:,i) + K(:,j) * e(j);
 277                                 % update covariance matrix
 278                                 P = (eye(6) - (K(:,j) * H(j,:))) * P;
 279                         end
 280
 281                         Bx = x(4,i);
 282                         By = x(5,i);
 283                         Bz = x(6,i);
 284                         gyro_bias(:,i) = [x1; x2; x3] * PI_DEG + [Bx; By; Bz];
 285                 end
 286         end
 287
 288         if GYRO_DATA_DISABLED == 1
 289                 quat_driv = quat_aid;
 290         end
 291
 292         if PLOT_SCALED_SENSOR_COMPARISON_DATA == 1
 293                 % Accelerometer/Gyroscope scaled Plot results
 294                 hfig=(figure);
 295                 scrsz = get(0,'ScreenSize');
 296                 set(hfig,'position',scrsz);
 297                 subplot(3,1,1)
 298                 p1 = plot(1:BUFFER_SIZE,acc_x(1,1:BUFFER_SIZE),'r');
 299                 hold on;
 300                 grid on;
 301                 if GYRO_DATA_DISABLED != 1
 302                         p2 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'b');
 303                         if MAG_DATA_DISABLED != 1
 304                                 hold on;
 305                                 grid on;
 306                                 p3 = plot(1:BUFFER_SIZE,mag_x(1,1:BUFFER_SIZE),'k');
 307                                 title(['Accelerometer/Gyroscope/Magnetometer X-Axis Plot']);
 308                                 legend([p1 p2 p3],'Acc_X', 'Gyr_X', 'Mag_X');
 309                         else
 310                                 title(['Accelerometer/Gyroscope X-Axis Plot']);
 311                                 legend([p1 p2],'Acc_X', 'Gyr_X');
 312                         end
 313                 else
 314                         p2 = plot(1:BUFFER_SIZE,mag_x(1,1:BUFFER_SIZE),'k');
 315                         title(['Accelerometer/Magnetometer X-Axis Plot']);
 316                         legend([p1 p2],'Acc_X', 'Mag_X');
 317                 end
 318                 subplot(3,1,2)
 319                 p1 = plot(1:BUFFER_SIZE,acc_y(1,1:BUFFER_SIZE),'r');
 320                 hold on;
 321                 grid on;
 322                 if GYRO_DATA_DISABLED != 1
 323                         p2 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'b');
 324                         if MAG_DATA_DISABLED != 1
 325                                 hold on;
 326                                 grid on;
 327                                 p3 = plot(1:BUFFER_SIZE,mag_y(1,1:BUFFER_SIZE),'k');
 328                                 title(['Accelerometer/Gyroscope/Magnetometer Y-Axis Plot']);
 329                                 legend([p1 p2 p3],'Acc_Y', 'Gyr_Y', 'Mag_Y');
 330                         else
 331                                 title(['Accelerometer/Gyroscope Y-Axis Plot']);
 332                                 legend([p1 p2],'Acc_Y', 'Gyr_Y');
 333                         end
 334                 else
 335                         p2 = plot(1:BUFFER_SIZE,mag_y(1,1:BUFFER_SIZE),'k');
 336                         title(['Accelerometer/Magnetometer Y-Axis Plot']);
 337                         legend([p1 p2],'Acc_X', 'Mag_Y');
 338                 end
 339                 subplot(3,1,3)
 340                 p1 = plot(1:BUFFER_SIZE,acc_z(1,1:BUFFER_SIZE),'r');
 341                 hold on;
 342                 grid on;
 343                 if GYRO_DATA_DISABLED != 1
 344                         p2 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'b');
 345                         if MAG_DATA_DISABLED != 1
 346                                 hold on;
 347                                 grid on;
 348                                 p3 = plot(1:BUFFER_SIZE,mag_z(1,1:BUFFER_SIZE),'k');
 349                                 title(['Accelerometer/Gyroscope/Magnetometer Z-Axis Plot']);
 350                                 legend([p1 p2 p3],'Acc_Z', 'Gyr_Z', 'Mag_Z');
 351                         else
 352                                 title(['Accelerometer/Gyroscope Z-Axis Plot']);
 353                                 legend([p1 p2],'Acc_Z', 'Gyr_Z');
 354                         end
 355                 else
 356                         p2 = plot(1:BUFFER_SIZE,mag_z(1,1:BUFFER_SIZE),'k');
 357                         title(['Accelerometer/Magnetometer Z-Axis Plot']);
 358                         legend([p1 p2],'Acc_Z', 'Mag_Z');
 359                 end
 360         end
 361
 362         if PLOT_INDIVIDUAL_SENSOR_INPUT_DATA == 1
 363                 % Accelerometer Raw (vs) filtered output
 364                 hfig=(figure);
 365                 scrsz = get(0,'ScreenSize');
 366                 set(hfig,'position',scrsz);
 367                 subplot(3,1,1)
 368                 p1 = plot(1:BUFFER_SIZE,Ax(1,1:BUFFER_SIZE),'r');
 369                 hold on;
 370                 grid on;
 371                 title(['Accelerometer X-Axis Plot']);
 372                 subplot(3,1,2)
 373                 p1 = plot(1:BUFFER_SIZE,Ay(1,1:BUFFER_SIZE),'r');
 374                 hold on;
 375                 grid on;
 376                 title(['Accelerometer Y-Axis Plot']);
 377                 subplot(3,1,3)
 378                 p1 = plot(1:BUFFER_SIZE,Az(1,1:BUFFER_SIZE),'r');
 379                 hold on;
 380                 grid on;
 381                 title(['Accelerometer Z-Axis Plot']);
 382
 383                 if GYRO_DATA_DISABLED != 1
 384                         % Gyroscope Raw (vs) filtered output
 385                         hfig=(figure);
 386                         scrsz = get(0,'ScreenSize');
 387                         set(hfig,'position',scrsz);
 388                         subplot(3,1,1)
 389                         p1 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'r');
 390                         hold on;
 391                         grid on;
 392                         title(['Gyroscope X-Axis Plot']);
 393                         subplot(3,1,2)
 394                         p1 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'r');
 395                         hold on;
 396                         grid on;
 397                         title(['Gyroscope Y-Axis Plot']);
 398                         subplot(3,1,3)
 399                         p1 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'r');
 400                         hold on;
 401                         grid on;
 402                         title(['Gyroscope Z-Axis Plot']);
 403                 end
 404
 405                 if MAG_DATA_DISABLED != 1
 406                         % Magnetometer Raw (vs) filtered output
 407                         hfig=(figure);
 408                         scrsz = get(0,'ScreenSize');
 409                         set(hfig,'position',scrsz);
 410                         subplot(3,1,1)
 411                         p1 = plot(1:BUFFER_SIZE,Mx(1,1:BUFFER_SIZE),'r');
 412                         hold on;
 413                         grid on;
 414                         title(['Magnetometer X-Axis Plot']);
 415                         subplot(3,1,2)
 416                         p1 = plot(1:BUFFER_SIZE,My(1,1:BUFFER_SIZE),'r');
 417                         hold on;
 418                         grid on;
 419                         title(['Magnetometer Y-Axis Plot']);
 420                         subplot(3,1,3)
 421                         p1 = plot(1:BUFFER_SIZE,Mz(1,1:BUFFER_SIZE),'r');
 422                         hold on;
 423                         grid on;
 424                         title(['Magnetometer Z-Axis Plot']);
 425                 end
 426         end
 427 end