% - Quaternion based approach\r
% - Estimation and correction of Euler errors and bias errors for gyroscope using Kalman filter\r
\r
-\r
+addpath('lib'); \r
clear\r
+clc\r
\r
LOW_PASS_FILTERING_ON = 1;\r
\r
%ZigmaW = 0; %deg/s \r
\r
% get accel x,y,z axis data from stored file\r
-Ax = (((dlmread("sensor_data/100ms/roll_pitch_yaw/accel.txt")(:,1))') - Bias_Ax)(1:BUFFER_SIZE);\r
-Ay = (((dlmread("sensor_data/100ms/roll_pitch_yaw/accel.txt")(:,2))') - Bias_Ay)(1:BUFFER_SIZE);\r
-Az = (((dlmread("sensor_data/100ms/roll_pitch_yaw/accel.txt")(:,3))') - Bias_Az)(1:BUFFER_SIZE);\r
-ATime = ((dlmread("sensor_data/100ms/roll_pitch_yaw/accel.txt")(:,4))');\r
+Ax = (((dlmread("data/100ms/roll_pitch_yaw/accel.txt")(:,1))') - Bias_Ax)(1:BUFFER_SIZE);\r
+Ay = (((dlmread("data/100ms/roll_pitch_yaw/accel.txt")(:,2))') - Bias_Ay)(1:BUFFER_SIZE);\r
+Az = (((dlmread("data/100ms/roll_pitch_yaw/accel.txt")(:,3))') - Bias_Az)(1:BUFFER_SIZE);\r
+ATime = ((dlmread("data/100ms/roll_pitch_yaw/accel.txt")(:,4))');\r
\r
% get gyro x,y,z axis data from stored file\r
-Gx = (((dlmread("sensor_data/100ms/roll_pitch_yaw/gyro.txt")(:,1))') - Bias_Gx)(1:BUFFER_SIZE);\r
-Gy = (((dlmread("sensor_data/100ms/roll_pitch_yaw/gyro.txt")(:,2))') - Bias_Gy)(1:BUFFER_SIZE);\r
-Gz = (((dlmread("sensor_data/100ms/roll_pitch_yaw/gyro.txt")(:,3))') - Bias_Gz)(1:BUFFER_SIZE);\r
-GTime = ((dlmread("sensor_data/100ms/roll_pitch_yaw/gyro.txt")(:,4))');\r
+Gx = (((dlmread("data/100ms/roll_pitch_yaw/gyro.txt")(:,1))') - Bias_Gx)(1:BUFFER_SIZE);\r
+Gy = (((dlmread("data/100ms/roll_pitch_yaw/gyro.txt")(:,2))') - Bias_Gy)(1:BUFFER_SIZE);\r
+Gz = (((dlmread("data/100ms/roll_pitch_yaw/gyro.txt")(:,3))') - Bias_Gz)(1:BUFFER_SIZE);\r
+GTime = ((dlmread("data/100ms/roll_pitch_yaw/gyro.txt")(:,4))');\r
\r
scale_Gyro = 575;\r
Gx = Gx/scale_Gyro;\r
Gz = Gz/scale_Gyro;\r
\r
% get magnetometer x,y,z axis data from stored file\r
-Mx = (((dlmread("sensor_data/100ms/roll_pitch_yaw/magnetic.txt")(:,1))'))(1:BUFFER_SIZE);\r
-My = (((dlmread("sensor_data/100ms/roll_pitch_yaw/magnetic.txt")(:,2))'))(1:BUFFER_SIZE);\r
-Mz = (((dlmread("sensor_data/100ms/roll_pitch_yaw/magnetic.txt")(:,3))'))(1:BUFFER_SIZE);\r
-MTime = ((dlmread("sensor_data/100ms/roll_pitch_yaw/magnetic.txt")(:,4))');\r
+Mx = (((dlmread("data/100ms/roll_pitch_yaw/magnetic.txt")(:,1))'))(1:BUFFER_SIZE);\r
+My = (((dlmread("data/100ms/roll_pitch_yaw/magnetic.txt")(:,2))'))(1:BUFFER_SIZE);\r
+Mz = (((dlmread("data/100ms/roll_pitch_yaw/magnetic.txt")(:,3))'))(1:BUFFER_SIZE);\r
+MTime = ((dlmread("data/100ms/roll_pitch_yaw/magnetic.txt")(:,4))');\r
\r
% Gyroscope Bias Variables\r
Bx = 0; By = 0; Bz = 0;\r