Modifying geomagnetic_RV files for new estimate_orientation

- Adapting geomagnetic_RV octave files to the restructured
  estmate_orientation ocateve implementation
- cleanup

Change-Id: I84ce43db89cddbb7f664b67608e90ef9c2068d99

diff --git a/src/sensor_fusion/design/lib/estimate_geomagnetic_rv.m b/src/sensor_fusion/design/lib/estimate_geomagnetic_rv.m
index a3ac4da..340f4d0 100755 (executable)
--- a/src/sensor_fusion/design/lib/estimate_geomagnetic_rv.m
+++ b/src/sensor_fusion/design/lib/estimate_geomagnetic_rv.m
@@ -26,16 +26,12 @@ function [Quat_aid]  = estimate_geomagnetic_rv(Accel_data, Mag_data)
 GRAVITY = 9.80665;
 RAD2DEG = 57.2957795;
 
-PITCH_PHASE_CORRECTION = -1;
-ROLL_PHASE_CORRECTION = -1;
-YAW_PHASE_CORRECTION = -1;
-
 BUFFER_SIZE = size(Accel_data,2);
 
 Gravity = zeros(3,BUFFER_SIZE);
 
 OR_driv = zeros(3,BUFFER_SIZE);
-Gyro_data = zeros(3,BUFFER_SIZE);
+Gyro_data = zeros(4,BUFFER_SIZE);
 
 Quat_driv = zeros(4,BUFFER_SIZE);
 Quat_aid = zeros(4,BUFFER_SIZE);

diff --git a/src/sensor_fusion/design/sf_geomagnetic_rv.m b/src/sensor_fusion/design/sf_geomagnetic_rv.m
index 595ca75..78802eb 100755 (executable)
--- a/src/sensor_fusion/design/sf_geomagnetic_rv.m
+++ b/src/sensor_fusion/design/sf_geomagnetic_rv.m
@@ -42,7 +42,7 @@ Sign_Mx = 1;
 Sign_My = 1;
 Sign_Mz = 1;
 
-BUFFER_SIZE = 100;
+BUFFER_SIZE = 1095;
 
 Accel_data = zeros(4,BUFFER_SIZE);
 Mag_data =  zeros(4,BUFFER_SIZE);
@@ -67,13 +67,14 @@ Mag_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(
 % estimate orientation
 Geo_RV = estimate_geomagnetic_rv(Accel_data, Mag_data);
 
-Orientation_RV = quat2euler(Geo_RV);
+for i = 1:BUFFER_SIZE
+       Orientation_RV(:,i) = quat2euler(Geo_RV(i,:));
+end
 
 hfig=(figure);
 scrsz = get(0,'ScreenSize');
 set(hfig,'position',scrsz);
 % Geomagnetic Rotation Vector Plot Results
-subplot(2,1,2)
 UA = Orientation_RV(1,:);
 p1 = plot(1:length(UA),UA(1,1:length(UA)),'k');
 hold on;
@@ -87,6 +88,3 @@ p3 = plot(1:length(UA),UA(1,1:length(UA)),'r');
 title(['Geomagnetic Rotation Vector']);
 legend([p1 p2 p3],'x-axis', 'y-axis', 'z-axis');
 
-
-
-