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