Removing low pass filtering from design

- low pass filtering has already been removed from sensor fusion
implementation to improve estimated orientation response speed.
- updating the same in octave design

Change-Id: I1b9a3ea985424a9fa7db28655b11f674daa5ae75

diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m
index 6e96c89..d20ac6c 100755 (executable)
--- a/src/sensor_fusion/design/lib/estimate_orientation.m
+++ b/src/sensor_fusion/design/lib/estimate_orientation.m
@@ -68,21 +68,6 @@ function [OR_driv, OR_aid, OR_err]  = estimate_orientation(Accel_data, Gyro_data
        % Gyroscope Bias Variables
        Bx = 0; By = 0; Bz = 0;
 
-       % 1st order smoothening filter
-       b = [0.98   0 ];
-       a = [1.0000000  0.02 ];
-
-       % initialize filter output variables to zero
-       filt_Ax = zeros(1,BUFFER_SIZE);
-       filt_Ay = zeros(1,BUFFER_SIZE);
-       filt_Az = zeros(1,BUFFER_SIZE);
-       filt_Gx = zeros(1,BUFFER_SIZE);
-       filt_Gy = zeros(1,BUFFER_SIZE);
-       filt_Gz = zeros(1,BUFFER_SIZE);
-       filt_Mx = zeros(1,BUFFER_SIZE);
-       filt_My = zeros(1,BUFFER_SIZE);
-       filt_Mz = zeros(1,BUFFER_SIZE);
-
        acc_x = zeros(1,BUFFER_SIZE);
        acc_y = zeros(1,BUFFER_SIZE);
        acc_z = zeros(1,BUFFER_SIZE);
@@ -139,55 +124,21 @@ function [OR_driv, OR_aid, OR_err]  = estimate_orientation(Accel_data, Gyro_data
 
        % first order filtering
        for i = 1:BUFFER_SIZE
-               if LOW_PASS_FILTERING_ON == 1
-                       if i < 2
-                               filt_Ax(i) = Ax(i);
-                               filt_Ay(i) = Ay(i);
-                               filt_Az(i) = Az(i);
-                               filt_Gx(i) = Gx(i);
-                               filt_Gy(i) = Gy(i);
-                               filt_Gz(i) = Gz(i);
-                               filt_Mx(i) = Mx(i);
-                               filt_My(i) = My(i);
-                               filt_Mz(i) = Mz(i);
-                       elseif i >= 2
-                               filt_Ax(i) = -a(2)*filt_Ax(i-1)+b(1)*Ax(i);
-                               filt_Ay(i) = -a(2)*filt_Ay(i-1)+b(1)*Ay(i);
-                               filt_Az(i) = -a(2)*filt_Az(i-1)+b(1)*Az(i);
-                               filt_Gx(i) = -a(2)*filt_Gx(i-1)+b(1)*Gx(i);
-                               filt_Gy(i) = -a(2)*filt_Gy(i-1)+b(1)*Gy(i);
-                               filt_Gz(i) = -a(2)*filt_Gz(i-1)+b(1)*Gz(i);
-                               filt_Mx(i) = -a(2)*filt_Mx(i-1)+b(1)*Mx(i);
-                               filt_My(i) = -a(2)*filt_My(i-1)+b(1)*My(i);
-                               filt_Mz(i) = -a(2)*filt_Mz(i-1)+b(1)*Mz(i);
-                       end
-               else
-                       filt_Ax(i) = Ax(i);
-                       filt_Ay(i) = Ay(i);
-                       filt_Az(i) = Az(i);
-                       filt_Gx(i) = Gx(i);
-                       filt_Gy(i) = Gy(i);
-                       filt_Gz(i) = Gz(i);
-                       filt_Mx(i) = Mx(i);
-                       filt_My(i) = My(i);
-                       filt_Mz(i) = Mz(i);
-               end
-
                % normalize accelerometer measurements
-               norm_acc = 1/sqrt(filt_Ax(i)^2 + filt_Ay(i)^2 + filt_Az(i)^2);
-               acc_x(i) = norm_acc * filt_Ax(i);
-               acc_y(i) = norm_acc * filt_Ay(i);
-               acc_z(i) = norm_acc * filt_Az(i);
+               norm_acc = 1/sqrt(Ax(i)^2 + Ay(i)^2 + Az(i)^2);
+               acc_x(i) = norm_acc * Ax(i);
+               acc_y(i) = norm_acc * Ay(i);
+               acc_z(i) = norm_acc * Az(i);
 
                % normalize magnetometer measurements
-               norm_mag = 1/sqrt(filt_Mx(i)^2 + filt_My(i)^2 + filt_Mz(i)^2);
-               mag_x(i) = norm_mag * filt_Mx(i);
-               mag_y(i) = norm_mag * filt_My(i);
-               mag_z(i) = norm_mag * filt_Mz(i);
+               norm_mag = 1/sqrt(Mx(i)^2 + My(i)^2 + Mz(i)^2);
+               mag_x(i) = norm_mag * Mx(i);
+               mag_y(i) = norm_mag * My(i);
+               mag_z(i) = norm_mag * Mz(i);
 
-               gyr_x(i) = filt_Gx(i) * PI;
-               gyr_y(i) = filt_Gy(i) * PI;
-               gyr_z(i) = filt_Gz(i) * PI;
+               gyr_x(i) = Gx(i) * PI;
+               gyr_y(i) = Gy(i) * PI;
+               gyr_z(i) = Gz(i) * PI;
 
                UA(i) = sqrt(acc_x(i)^2 + acc_y(i)^2 + acc_z(i)^2) - GRAVITY;
 
@@ -325,7 +276,7 @@ function [OR_driv, OR_aid, OR_err]  = estimate_orientation(Accel_data, Gyro_data
                p1 = plot(1:BUFFER_SIZE,acc_x(1,1:BUFFER_SIZE),'r');
                hold on;
                grid on;
-               p2 = plot(1:BUFFER_SIZE,filt_Gx(1,1:BUFFER_SIZE),'b');
+               p2 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'b');
                hold on;
                grid on;
                p3 = plot(1:BUFFER_SIZE,mag_x(1,1:BUFFER_SIZE),'k');
@@ -335,7 +286,7 @@ function [OR_driv, OR_aid, OR_err]  = estimate_orientation(Accel_data, Gyro_data
                p1 = plot(1:BUFFER_SIZE,acc_y(1,1:BUFFER_SIZE),'r');
                hold on;
                grid on;
-               p2 = plot(1:BUFFER_SIZE,filt_Gy(1,1:BUFFER_SIZE),'b');
+               p2 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'b');
                hold on;
                grid on;
                p3 = plot(1:BUFFER_SIZE,mag_y(1,1:BUFFER_SIZE),'k');
@@ -345,7 +296,7 @@ function [OR_driv, OR_aid, OR_err]  = estimate_orientation(Accel_data, Gyro_data
                p1 = plot(1:BUFFER_SIZE,acc_z(1,1:BUFFER_SIZE),'r');
                hold on;
                grid on;
-               p2 = plot(1:BUFFER_SIZE,filt_Gz(1,1:BUFFER_SIZE),'b');
+               p2 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'b');
                hold on;
                grid on;
                p3 = plot(1:BUFFER_SIZE,mag_z(1,1:BUFFER_SIZE),'k');
@@ -362,21 +313,21 @@ function [OR_driv, OR_aid, OR_err]  = estimate_orientation(Accel_data, Gyro_data
                p1 = plot(1:BUFFER_SIZE,Ax(1,1:BUFFER_SIZE),'r');
                hold on;
                grid on;
-               p2 = plot(1:BUFFER_SIZE,filt_Ax(1,1:BUFFER_SIZE),'b');
+               p2 = plot(1:BUFFER_SIZE,Ax(1,1:BUFFER_SIZE),'b');
                title(['Accelerometer X-Axis Plot']);
                legend([p1 p2],'input signal','low-pass filtered signal');
                subplot(3,1,2)
                p1 = plot(1:BUFFER_SIZE,Ay(1,1:BUFFER_SIZE),'r');
                hold on;
                grid on;
-               p2 = plot(1:BUFFER_SIZE,filt_Ay(1,1:BUFFER_SIZE),'b');
+               p2 = plot(1:BUFFER_SIZE,Ay(1,1:BUFFER_SIZE),'b');
                title(['Accelerometer Y-Axis Plot']);
                legend([p1 p2],'input signal','low-pass filtered signal');
                subplot(3,1,3)
                p1 = plot(1:BUFFER_SIZE,Az(1,1:BUFFER_SIZE),'r');
                hold on;
                grid on;
-               p2 = plot(1:BUFFER_SIZE,filt_Az(1,1:BUFFER_SIZE),'b');
+               p2 = plot(1:BUFFER_SIZE,Az(1,1:BUFFER_SIZE),'b');
                title(['Accelerometer Z-Axis Plot']);
                legend([p1 p2],'input signal','low-pass filtered signal');
 
@@ -388,21 +339,21 @@ function [OR_driv, OR_aid, OR_err]  = estimate_orientation(Accel_data, Gyro_data
                p1 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'r');
                hold on;
                grid on;
-               p2 = plot(1:BUFFER_SIZE,filt_Gx(1,1:BUFFER_SIZE),'b');
+               p2 = plot(1:BUFFER_SIZE,Gx(1,1:BUFFER_SIZE),'b');
                title(['Gyroscope X-Axis Plot']);
                legend([p1 p2],'input signal','low-pass filtered signal');
                subplot(3,1,2)
                p1 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'r');
                hold on;
                grid on;
-               p2 = plot(1:BUFFER_SIZE,filt_Gy(1,1:BUFFER_SIZE),'b');
+               p2 = plot(1:BUFFER_SIZE,Gy(1,1:BUFFER_SIZE),'b');
                title(['Gyroscope Y-Axis Plot']);
                legend([p1 p2],'input signal','low-pass filtered signal');
                subplot(3,1,3)
                p1 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'r');
                hold on;
                grid on;
-               p2 = plot(1:BUFFER_SIZE,filt_Gz(1,1:BUFFER_SIZE),'b');
+               p2 = plot(1:BUFFER_SIZE,Gz(1,1:BUFFER_SIZE),'b');
                title(['Gyroscope Z-Axis Plot']);
                legend([p1 p2],'input signal','low-pass filtered signal');
 
@@ -414,21 +365,21 @@ function [OR_driv, OR_aid, OR_err]  = estimate_orientation(Accel_data, Gyro_data
                p1 = plot(1:BUFFER_SIZE,Mx(1,1:BUFFER_SIZE),'r');
                hold on;
                grid on;
-               p2 = plot(1:BUFFER_SIZE,filt_Mx(1,1:BUFFER_SIZE),'b');
+               p2 = plot(1:BUFFER_SIZE,Mx(1,1:BUFFER_SIZE),'b');
                title(['Magnetometer X-Axis Plot']);
                legend([p1 p2],'input signal','low-pass filtered signal');
                subplot(3,1,2)
                p1 = plot(1:BUFFER_SIZE,My(1,1:BUFFER_SIZE),'r');
                hold on;
                grid on;
-               p2 = plot(1:BUFFER_SIZE,filt_My(1,1:BUFFER_SIZE),'b');
+               p2 = plot(1:BUFFER_SIZE,My(1,1:BUFFER_SIZE),'b');
                title(['Magnetometer Y-Axis Plot']);
                legend([p1 p2],'input signal','low-pass filtered signal');
                subplot(3,1,3)
                p1 = plot(1:BUFFER_SIZE,Mz(1,1:BUFFER_SIZE),'r');
                hold on;
                grid on;
-               p2 = plot(1:BUFFER_SIZE,filt_Mz(1,1:BUFFER_SIZE),'b');
+               p2 = plot(1:BUFFER_SIZE,Mz(1,1:BUFFER_SIZE),'b');
                title(['Magnetometer Z-Axis Plot']);
                legend([p1 p2],'input signal','low-pass filtered signal');
        end