SensorFusion/SF_Orien.m

   1 % SF_Orien\r
   2 %\r
   3 % Copyright (c) 2014 Samsung Electronics Co., Ltd.\r
   4 %\r
   5 % Licensed under the Apache License, Version 2.0 (the "License");\r
   6 % you may not use this file except in compliance with the License.\r
   7 % You may obtain a copy of the License at\r
   8 %\r
   9 % http://www.apache.org/licenses/LICENSE-2.0\r
  10 %\r
  11 % Unless required by applicable law or agreed to in writing, software\r
  12 % distributed under the License is distributed on an "AS IS" BASIS,\r
  13 % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\r
  14 % See the License for the specific language governing permissions and\r
  15 % limitations under the License.\r
  16 \r
  17 % Sensor Fusion Implementation - Orientation Estimation \r
  18 %\r
  19 % - Orientation Estimation using Gyroscope as the driving system and Accelerometer+Geo-Magnetic Sensors as Aiding System.\r
  20 % - Quaternion based approach\r
  21 % - Estimation and correction of Euler errors and bias errors for gyroscope using Kalman filter\r
  22 \r
  23 addpath('lib'); \r
  24 clear\r
  25 clc\r
  26 \r
  27 LOW_PASS_FILTERING_ON = 1;\r
  28 \r
  29 Max_Range_Accel = 39.203407; Min_Range_Accel = -39.204006; Res_Accel = 0.000598;\r
  30 Max_Range_Gyro = 1146.862549; Min_Range_Gyro = -1146.880005; Res_Gyro = 0.017500;\r
  31 Max_Range_Magnetic = 1200; Min_Range_Magnetic = -1200; Res_Magnetic = 1;\r
  32 \r
  33 PI = 3.141593;\r
  34 GRAVITY = 9.80665;\r
  35 NON_ZERO_VAL = 0.1;\r
  36 \r
  37 Bias_Ax = 0.098586;\r
  38 Bias_Ay = 0.18385;\r
  39 Bias_Az = 10.084 - GRAVITY;\r
  40 \r
  41 Bias_Gx = -5.3539;\r
  42 Bias_Gy = 0.24325;\r
  43 Bias_Gz = 2.3391;\r
  44 \r
  45 BUFFER_SIZE = 1095;\r
  46 MOVING_AVERAGE_WINDOW_LENGTH = 20;\r
  47 RAD2DEG = 57.2957795;\r
  48 DEG2RAD = 0.0174532925;\r
  49 US2S =  1.0 / 1000000.0;\r
  50 \r
  51 % Gyro Types\r
  52 % Systron-donner "Horizon"\r
  53 ZigmaW = 0.05 * DEG2RAD; %deg/s \r
  54 TauW = 1000; %secs \r
  55 % Crossbow DMU-6X\r
  56 %ZigmaW = 0.05 * DEG2RAD; %deg/s \r
  57 %TauW = 300; %secs \r
  58 %FOGs (KVH Autogyro and Crossbow DMU-FOG)\r
  59 %ZigmaW = 0; %deg/s  \r
  60 \r
  61 % get accel x,y,z axis data from stored file\r
  62 Ax = (((dlmread("data/100ms/roll_pitch_yaw/accel.txt")(:,1))') - Bias_Ax)(1:BUFFER_SIZE);\r
  63 Ay = (((dlmread("data/100ms/roll_pitch_yaw/accel.txt")(:,2))') - Bias_Ay)(1:BUFFER_SIZE);\r
  64 Az = (((dlmread("data/100ms/roll_pitch_yaw/accel.txt")(:,3))') - Bias_Az)(1:BUFFER_SIZE);\r
  65 ATime = ((dlmread("data/100ms/roll_pitch_yaw/accel.txt")(:,4))');\r
  66 \r
  67 % get gyro x,y,z axis data from stored file\r
  68 Gx = (((dlmread("data/100ms/roll_pitch_yaw/gyro.txt")(:,1))') - Bias_Gx)(1:BUFFER_SIZE);\r
  69 Gy = (((dlmread("data/100ms/roll_pitch_yaw/gyro.txt")(:,2))') - Bias_Gy)(1:BUFFER_SIZE);\r
  70 Gz = (((dlmread("data/100ms/roll_pitch_yaw/gyro.txt")(:,3))') - Bias_Gz)(1:BUFFER_SIZE);\r
  71 GTime = ((dlmread("data/100ms/roll_pitch_yaw/gyro.txt")(:,4))');\r
  72 \r
  73 scale_Gyro = 575;\r
  74 Gx = Gx/scale_Gyro;\r
  75 Gy = Gy/scale_Gyro;\r
  76 Gz = Gz/scale_Gyro;\r
  77 \r
  78 % get magnetometer x,y,z axis data from stored file\r
  79 Mx = (((dlmread("data/100ms/roll_pitch_yaw/magnetic.txt")(:,1))'))(1:BUFFER_SIZE);\r
  80 My = (((dlmread("data/100ms/roll_pitch_yaw/magnetic.txt")(:,2))'))(1:BUFFER_SIZE);\r
  81 Mz = (((dlmread("data/100ms/roll_pitch_yaw/magnetic.txt")(:,3))'))(1:BUFFER_SIZE);\r
  82 MTime = ((dlmread("data/100ms/roll_pitch_yaw/magnetic.txt")(:,4))');\r
  83 \r
  84 % Gyroscope Bias Variables\r
  85 Bx = 0; By = 0; Bz = 0;\r
  86 \r
  87 % 1st order smoothening filter\r
  88 b = [0.98   0 ];\r
  89 a = [1.0000000  0.02 ];\r
  90 \r
  91 % initialize filter output variables to zero\r
  92 filt_Ax = zeros(1,BUFFER_SIZE);\r
  93 filt_Ay = zeros(1,BUFFER_SIZE);\r
  94 filt_Az = zeros(1,BUFFER_SIZE);\r
  95 filt_Gx = zeros(1,BUFFER_SIZE);\r
  96 filt_Gy = zeros(1,BUFFER_SIZE);\r
  97 filt_Gz = zeros(1,BUFFER_SIZE);\r
  98 filt_Mx = zeros(1,BUFFER_SIZE);\r
  99 filt_My = zeros(1,BUFFER_SIZE);\r
 100 filt_Mz = zeros(1,BUFFER_SIZE);\r
 101 \r
 102 acc_x = zeros(1,BUFFER_SIZE);\r
 103 acc_y = zeros(1,BUFFER_SIZE);\r
 104 acc_z = zeros(1,BUFFER_SIZE);\r
 105 gyr_x = zeros(1,BUFFER_SIZE);\r
 106 gyr_y = zeros(1,BUFFER_SIZE);\r
 107 gyr_z = zeros(1,BUFFER_SIZE);\r
 108 mag_x = zeros(1,BUFFER_SIZE);\r
 109 mag_y = zeros(1,BUFFER_SIZE);\r
 110 mag_z = zeros(1,BUFFER_SIZE);\r
 111 \r
 112 % User Acceleration mean and Variance\r
 113 A_T = zeros(1,BUFFER_SIZE);\r
 114 G_T = zeros(1,BUFFER_SIZE);\r
 115 M_T = zeros(1,BUFFER_SIZE);\r
 116 var_roll = zeros(1,BUFFER_SIZE);\r
 117 var_pitch = zeros(1,BUFFER_SIZE);\r
 118 var_yaw = zeros(1,BUFFER_SIZE);\r
 119 var_Gx = zeros(1,BUFFER_SIZE);\r
 120 var_Gy = zeros(1,BUFFER_SIZE);\r
 121 var_Gz = zeros(1,BUFFER_SIZE);\r
 122 \r
 123 roll = zeros(1,BUFFER_SIZE);\r
 124 pitch = zeros(1,BUFFER_SIZE);\r
 125 yaw = zeros(1,BUFFER_SIZE);\r
 126 quat_driv = zeros(BUFFER_SIZE,4);\r
 127 quat_aid = zeros(BUFFER_SIZE,4);\r
 128 quat_error = zeros(BUFFER_SIZE,4);\r
 129 euler = zeros(BUFFER_SIZE,3);\r
 130 Ro = zeros(3, 3, BUFFER_SIZE);\r
 131 \r
 132 % system covariance matrix\r
 133 Q = zeros(6,6);\r
 134 \r
 135 % measurement covariance matrix\r
 136 R = zeros(6,6);\r
 137         \r
 138 A_T(1) = 100000;\r
 139 G_T(1) = 100000;\r
 140 M_T(1) = 100000;\r
 141         \r
 142 acc_e = [0.0;0.0;1.0]; % gravity vector in earth frame\r
 143 mag_e = [0.0;1.0;0.0]; % magnetic field vector in earth frame   \r
 144         \r
 145 H = [eye(3) zeros(3,3); zeros(3,6)];\r
 146 x = zeros(6,BUFFER_SIZE);\r
 147 e = zeros(1,6);\r
 148 P = 1 * eye(6);% state covariance matrix\r
 149 \r
 150 quat_driv(1,:) = [1 0 0 0];\r
 151 \r
 152 % first order filtering\r
 153 for i = 1:BUFFER_SIZE\r
 154         if LOW_PASS_FILTERING_ON == 1\r
 155                 if i < 2\r
 156                         filt_Ax(i) = Ax(i);\r
 157                         filt_Ay(i) = Ay(i);\r
 158                         filt_Az(i) = Az(i);\r
 159                         filt_Gx(i) = Gx(i);\r
 160                         filt_Gy(i) = Gy(i);\r
 161                         filt_Gz(i) = Gz(i);\r
 162                         filt_Mx(i) = Mx(i);\r
 163                         filt_My(i) = My(i);\r
 164                         filt_Mz(i) = Mz(i);\r
 165                 elseif i >= 2\r
 166                         filt_Ax(i) = -a(2)*filt_Ax(i-1)+b(1)*Ax(i);\r
 167                         filt_Ay(i) = -a(2)*filt_Ay(i-1)+b(1)*Ay(i);\r
 168                         filt_Az(i) = -a(2)*filt_Az(i-1)+b(1)*Az(i);\r
 169                         filt_Gx(i) = -a(2)*filt_Gx(i-1)+b(1)*Gx(i);\r
 170                         filt_Gy(i) = -a(2)*filt_Gy(i-1)+b(1)*Gy(i);\r
 171                         filt_Gz(i) = -a(2)*filt_Gz(i-1)+b(1)*Gz(i);\r
 172                         filt_Mx(i) = -a(2)*filt_Mx(i-1)+b(1)*Mx(i);\r
 173                         filt_My(i) = -a(2)*filt_My(i-1)+b(1)*My(i);\r
 174                         filt_Mz(i) = -a(2)*filt_Mz(i-1)+b(1)*Mz(i);\r
 175                 end\r
 176         else\r
 177                 filt_Ax(i) = Ax(i);\r
 178                 filt_Ay(i) = Ay(i);\r
 179                 filt_Az(i) = Az(i);\r
 180                 filt_Gx(i) = Gx(i);\r
 181                 filt_Gy(i) = Gy(i);\r
 182                 filt_Gz(i) = Gz(i);\r
 183                 filt_Mx(i) = Mx(i);\r
 184                 filt_My(i) = My(i);\r
 185                 filt_Mz(i) = Mz(i);\r
 186         end\r
 187         \r
 188         % normalize accelerometer measurements\r
 189         norm_acc = 1/sqrt(filt_Ax(i)^2 + filt_Ay(i)^2 + filt_Az(i)^2);\r
 190         acc_x(i) = norm_acc * filt_Ax(i);\r
 191         acc_y(i) = norm_acc * filt_Ay(i);\r
 192         acc_z(i) = norm_acc * filt_Az(i);\r
 193 \r
 194         % normalize magnetometer measurements\r
 195         norm_mag = 1/sqrt(filt_Mx(i)^2 + filt_My(i)^2 + filt_Mz(i)^2);\r
 196         mag_x(i) = norm_mag * filt_Mx(i);\r
 197         mag_y(i) = norm_mag * filt_My(i);\r
 198         mag_z(i) = norm_mag * filt_Mz(i);\r
 199         \r
 200         gyr_x(i) = filt_Gx(i) * PI;\r
 201         gyr_y(i) = filt_Gy(i) * PI;\r
 202         gyr_z(i) = filt_Gz(i) * PI;\r
 203         \r
 204         UA(i) = sqrt(acc_x(i)^2 + acc_y(i)^2 + acc_z(i)^2) - GRAVITY;\r
 205         \r
 206         gyr_x(i) = gyr_x(i) - Bx;\r
 207         gyr_y(i) = gyr_y(i) - By;\r
 208         gyr_z(i) = gyr_z(i) - Bz;\r
 209         \r
 210         % Aiding System (Accelerometer + Geomagnetic) quaternion generation\r
 211         % gravity vector in body frame\r
 212         acc_b =[acc_x(i);acc_y(i);acc_z(i)]; \r
 213         % magnetic field vector in body frame\r
 214         mag_b =[mag_x(i);mag_y(i);mag_z(i)];\r
 215 \r
 216         % compute measurement quaternion with TRIAD algorithm\r
 217         acc_b_x_mag_b = cross(acc_b,mag_b);\r
 218         acc_e_x_mag_e = cross(acc_e,mag_e);\r
 219         M_b = [acc_b acc_b_x_mag_b cross(acc_b_x_mag_b,acc_b)];\r
 220         M_e = [acc_e acc_e_x_mag_e cross(acc_e_x_mag_e,acc_e)];\r
 221         Rot_m = M_e * M_b';     \r
 222         quat_aid(i,:) = RotMat2Quat(Rot_m);\r
 223         \r
 224         euler = Quat2Euler(quat_aid(i,:));\r
 225         roll(i) = euler(1);\r
 226         pitch(i) = euler(2);\r
 227         yaw(i) = euler(3);\r
 228         \r
 229         if i <= MOVING_AVERAGE_WINDOW_LENGTH\r
 230                 var_Gx(i) = NON_ZERO_VAL;\r
 231                 var_Gy(i) = NON_ZERO_VAL;\r
 232                 var_Gz(i) = NON_ZERO_VAL;\r
 233                 var_roll(i) = NON_ZERO_VAL;\r
 234                 var_pitch(i) = NON_ZERO_VAL;\r
 235                 var_yaw(i) = NON_ZERO_VAL;\r
 236         else\r
 237                 var_Gx(i) = var(gyr_x((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
 238                 var_Gy(i) = var(gyr_y((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
 239                 var_Gz(i) = var(gyr_z((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
 240                 var_roll(i) = var(roll((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
 241                 var_pitch(i) = var(pitch((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
 242                 var_yaw(i) = var(yaw((i-MOVING_AVERAGE_WINDOW_LENGTH):i));\r
 243         end\r
 244         if i > 1\r
 245                 A_T(i) = ATime(i) - ATime(i-1);\r
 246                 G_T(i) = GTime(i) - GTime(i-1);\r
 247                 M_T(i) = MTime(i) - MTime(i-1);\r
 248         end\r
 249         \r
 250         dt = G_T(i) * US2S;\r
 251         \r
 252         Qwn = [var_Gx(i) 0 0;0 var_Gy(i) 0;0 0 var_Gz(i);];\r
 253         Qwb = (2 * (ZigmaW^2) / TauW) * eye(3);\r
 254         \r
 255         % Process Noise Covariance\r
 256         Q = [Qwn zeros(3,3);zeros(3,3) Qwb];\r
 257         \r
 258         % Measurement Noise Covariance\r
 259         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)];\r
 260         \r
 261         % initialization for q\r
 262         if i == 1\r
 263                 q = quat_aid(i,:);\r
 264         end\r
 265         \r
 266         q_t = [q(2) q(3) q(4)]';\r
 267         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];\r
 268         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))];\r
 269 \r
 270         % Time Update\r
 271         if i > 1\r
 272                 x(:,i) = F * x(:,i-1);\r
 273         end\r
 274 \r
 275         % compute covariance of prediction\r
 276         P = (F * P * F') + Q;\r
 277         \r
 278         % Driving System (Gyroscope) quaternion generation\r
 279         % convert scaled gyro data to rad/s\r
 280         qDot = 0.5 * QuatProd(q, [0 gyr_x(i) gyr_y(i) gyr_z(i)]);\r
 281 \r
 282     % Integrate to yield quaternion\r
 283     q = q + qDot * dt * PI;\r
 284         \r
 285         % normalise quaternion\r
 286     quat_driv(i,:) = q / norm(q);\r
 287 \r
 288         % Kalman Filtering\r
 289         quat_error(i,:) = QuatProd(quat_aid(i,:), quat_driv(i,:));\r
 290         \r
 291         euler_e = Quat2Euler(quat_error(i,:));\r
 292         x1 = euler_e(1)'/PI;\r
 293         x2 = euler_e(2)'/PI;\r
 294         x3 = euler_e(3)'/PI;\r
 295         \r
 296         q = QuatProd(quat_driv(i,:), [1 x1 x2 x3]) * PI;\r
 297         q = q / norm(q);\r
 298         \r
 299         Bx = Bx + x(4,i);\r
 300         By = By + x(5,i);\r
 301         Bz = Bz + x(6,i);\r
 302 \r
 303         if i > 1\r
 304                 e = [x1 x2 x3 x(4,i) x(5,i) x(6,i)];\r
 305         end\r
 306         \r
 307         for j =1:6\r
 308                 % compute Kalman gain\r
 309                 K(:,j) = P(j ,:)./(P(j,j)+R(j,j));\r
 310                 % update state vector\r
 311                 x(:,i) = x(:,i) + K(:,j) * e(j);\r
 312                 % update covariance matrix\r
 313                 P = (eye(6) - (K(:,j) * H(j,:))) * P;\r
 314         end\r
 315 end\r
 316 \r
 317 \r
 318 roll = roll * RAD2DEG;\r
 319 pitch = pitch * RAD2DEG;\r
 320 yaw = yaw * RAD2DEG;\r
 321 \r
 322 euler = Quat2Euler(quat_driv);\r
 323 phi = euler(:,1)' * RAD2DEG;\r
 324 theta = euler(:,2)' * RAD2DEG;\r
 325 psi = -euler(:,3)' * RAD2DEG;\r
 326 \r
 327 euler = Quat2Euler(quat_error);\r
 328 roll_e = euler(:,1)' * RAD2DEG;\r
 329 pitch_e = euler(:,2)' * RAD2DEG;\r
 330 yaw_e = euler(:,3)' * RAD2DEG;\r
 331 \r
 332 % Plot results\r
 333 close all\r
 334 \r
 335 % Rotation Plot Results\r
 336 hfig=(figure);\r
 337 scrsz = get(0,'ScreenSize');\r
 338 set(hfig,'position',scrsz);\r
 339 subplot(3,1,1)\r
 340 UA = pitch;\r
 341 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');\r
 342 hold on;\r
 343 grid on;\r
 344 UA = theta;\r
 345 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');\r
 346 hold on;\r
 347 grid on;\r
 348 UA = pitch_e;\r
 349 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');\r
 350 title(['Pitch']);\r
 351 legend([p1 p2 p3],'Aiding System', 'Driving System', 'Quaternion based error');\r
 352 subplot(3,1,2)\r
 353 UA = roll;\r
 354 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');\r
 355 hold on;\r
 356 grid on;\r
 357 UA = phi;\r
 358 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');\r
 359 hold on;\r
 360 grid on;\r
 361 UA = roll_e;\r
 362 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');\r
 363 title(['Roll']);\r
 364 legend([p1 p2 p3],'Aiding System', 'Driving System', 'Quaternion based error');\r
 365 subplot(3,1,3)\r
 366 UA = yaw;\r
 367 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');\r
 368 hold on;\r
 369 grid on;\r
 370 UA = psi;\r
 371 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');\r
 372 hold on;\r
 373 grid on;\r
 374 UA = yaw_e;\r
 375 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');\r
 376 title(['Yaw']);\r
 377 legend([p1 p2 p3],'Aiding System', 'Driving System', 'Quaternion based error');\r
 378 \r
 379 % Accelerometer/Gyroscope/Magnetometer scaled Plot results\r
 380 hfig=(figure);\r
 381 scrsz = get(0,'ScreenSize');\r
 382 set(hfig,'position',scrsz);\r
 383 subplot(3,1,1)\r
 384 p1 = plot(1:BUFFER_SIZE,acc_x(1,1:BUFFER_SIZE),'r');\r
 385 hold on;\r
 386 grid on;\r
 387 p2 = plot(1:BUFFER_SIZE,gyr_x(1,1:BUFFER_SIZE),'b');\r
 388 hold on;\r
 389 grid on;\r
 390 p3 = plot(1:BUFFER_SIZE,mag_x(1,1:BUFFER_SIZE),'k');\r
 391 title(['Accelerometer/Gyroscope/Magnetometer X-Axis Plot']);\r
 392 legend([p1 p2 p3],'Acc_X', 'Gyr_X', 'Mag_X');\r
 393 subplot(3,1,2)\r
 394 p1 = plot(1:BUFFER_SIZE,acc_y(1,1:BUFFER_SIZE),'r');\r
 395 hold on;\r
 396 grid on;\r
 397 p2 = plot(1:BUFFER_SIZE,gyr_y(1,1:BUFFER_SIZE),'b');\r
 398 hold on;\r
 399 grid on;\r
 400 p3 = plot(1:BUFFER_SIZE,mag_y(1,1:BUFFER_SIZE),'k');\r
 401 title(['Accelerometer/Gyroscope/Magnetometer Y-Axis Plot']);\r
 402 legend([p1 p2 p3],'Acc_Y', 'Gyr_Y', 'Mag_Y');\r
 403 subplot(3,1,3)\r
 404 p1 = plot(1:BUFFER_SIZE,acc_z(1,1:BUFFER_SIZE),'r');\r
 405 hold on;\r
 406 grid on;\r
 407 p2 = plot(1:BUFFER_SIZE,gyr_z(1,1:BUFFER_SIZE),'b');\r
 408 hold on;\r
 409 grid on;\r
 410 p3 = plot(1:BUFFER_SIZE,mag_z(1,1:BUFFER_SIZE),'k');\r
 411 title(['Accelerometer/Gyroscope/Magnetometer Z-Axis Plot']);\r
 412 legend([p1 p2 p3],'Acc_Z', 'Gyr_Z', 'Mag_Z');\r
 413 \r
 414 % Accelerometer Raw (vs) filtered output\r
 415 hfig=(figure);\r
 416 scrsz = get(0,'ScreenSize');\r
 417 set(hfig,'position',scrsz);\r
 418 subplot(3,1,1)\r
 419 p1 = plot(1:BUFFER_SIZE,Ax(1,1:BUFFER_SIZE),'r');\r
 420 hold on;\r
 421 grid on;\r
 422 p2 = plot(1:BUFFER_SIZE,filt_Ax(1,1:BUFFER_SIZE),'b');\r
 423 title(['Accelerometer X-Axis Plot']);\r
 424 legend([p1 p2],'input signal','low-pass filtered signal');\r
 425 subplot(3,1,2)\r
 426 p1 = plot(1:BUFFER_SIZE,Ay(1,1:BUFFER_SIZE),'r');\r
 427 hold on;\r
 428 grid on;\r
 429 p2 = plot(1:BUFFER_SIZE,filt_Ay(1,1:BUFFER_SIZE),'b');\r
 430 title(['Accelerometer Y-Axis Plot']);\r
 431 legend([p1 p2],'input signal','low-pass filtered signal');\r
 432 subplot(3,1,3)\r
 433 p1 = plot(1:BUFFER_SIZE,Az(1,1:BUFFER_SIZE),'r');\r
 434 hold on;\r
 435 grid on;\r
 436 p2 = plot(1:BUFFER_SIZE,filt_Az(1,1:BUFFER_SIZE),'b');\r
 437 title(['Accelerometer Z-Axis Plot']);\r
 438 legend([p1 p2],'input signal','low-pass filtered signal');\r
 439 \r
 440 % Gyroscope Raw (vs) filtered output\r
 441 hfig=(figure);\r
 442 scrsz = get(0,'ScreenSize');\r
 443 set(hfig,'position',scrsz);\r
 444 subplot(3,1,1)\r
 445 p1 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'r');\r
 446 hold on;\r
 447 grid on;\r
 448 p2 = plot(1:BUFFER_SIZE,filt_Gx(1,1:BUFFER_SIZE),'b');\r
 449 title(['Gyroscope X-Axis Plot']);\r
 450 legend([p1 p2],'input signal','low-pass filtered signal');\r
 451 subplot(3,1,2)\r
 452 p1 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'r');\r
 453 hold on;\r
 454 grid on;\r
 455 p2 = plot(1:BUFFER_SIZE,filt_Gy(1,1:BUFFER_SIZE),'b');\r
 456 title(['Gyroscope Y-Axis Plot']);\r
 457 legend([p1 p2],'input signal','low-pass filtered signal');\r
 458 subplot(3,1,3)\r
 459 p1 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'r');\r
 460 hold on;\r
 461 grid on;\r
 462 p2 = plot(1:BUFFER_SIZE,filt_Gz(1,1:BUFFER_SIZE),'b');\r
 463 title(['Gyroscope Z-Axis Plot']);\r
 464 legend([p1 p2],'input signal','low-pass filtered signal');\r
 465 \r
 466 % Magnetometer Raw (vs) filtered output\r
 467 hfig=(figure);\r
 468 scrsz = get(0,'ScreenSize');\r
 469 set(hfig,'position',scrsz);\r
 470 subplot(3,1,1)\r
 471 p1 = plot(1:BUFFER_SIZE,Mx(1,1:BUFFER_SIZE),'r');\r
 472 hold on;\r
 473 grid on;\r
 474 p2 = plot(1:BUFFER_SIZE,filt_Mx(1,1:BUFFER_SIZE),'b');\r
 475 title(['Magnetometer X-Axis Plot']);\r
 476 legend([p1 p2],'input signal','low-pass filtered signal');\r
 477 subplot(3,1,2)\r
 478 p1 = plot(1:BUFFER_SIZE,My(1,1:BUFFER_SIZE),'r');\r
 479 hold on;\r
 480 grid on;\r
 481 p2 = plot(1:BUFFER_SIZE,filt_My(1,1:BUFFER_SIZE),'b');\r
 482 title(['Magnetometer Y-Axis Plot']);\r
 483 legend([p1 p2],'input signal','low-pass filtered signal');\r
 484 subplot(3,1,3)\r
 485 p1 = plot(1:BUFFER_SIZE,Mz(1,1:BUFFER_SIZE),'r');\r
 486 hold on;\r
 487 grid on;\r
 488 p2 = plot(1:BUFFER_SIZE,filt_Mz(1,1:BUFFER_SIZE),'b');\r
 489 title(['Magnetometer Z-Axis Plot']);\r
 490 legend([p1 p2],'input signal','low-pass filtered signal');\r
 491 \r