src/sensor_fusion/design/lib/estimate_orientation.m

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