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