src/sensor/rotation_vector/design/sf_geomagnetic_rv.m

   1 % sf_geomagnetic_rv
   2 %
   3 % Copyright (c) 2015 Samsung Electronics Co., Ltd.
   4 %
   5 % Licensed under the Apache License, Version 2.0 (the "License");
   6 % you may not use this file except in compliance with the License.
   7 % You may obtain a copy of the License at
   8 %
   9 % http://www.apache.org/licenses/LICENSE-2.0
  10 %
  11 % Unless required by applicable law or agreed to in writing, software
  12 % distributed under the License is distributed on an "AS IS" BASIS,
  13 % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14 % See the License for the specific language governing permissions and
  15 % limitations under the License.
  16
  17 % Sensor Fusion Implementation for Determination of Geomagentic rotation Vector
  18 %
  19 % - Input Accelerometer and Magnetometer sensor data
  20 % - Call estimate_geomagnetic_rotation
  21 % - Plot results for geomagetic rotation on reference axis
  22
  23 addpath('lib');
  24 clear
  25 close all
  26 clc
  27
  28 GRAVITY = 9.80665;
  29
  30 Max_Range_Accel = 39.203407; Min_Range_Accel = -39.204006; Res_Accel = 0.000598;
  31 Max_Range_Magnetic = 1200; Min_Range_Magnetic = -1200; Res_Magnetic = 1;
  32
  33 Bias_Ax = 0.098586;
  34 Bias_Ay = 0.18385;
  35 Bias_Az = 10.084 - GRAVITY;
  36
  37 Bias_Mx = 0;
  38 Bias_My = 0;
  39 Bias_Mz = 0;
  40
  41 Sign_Mx = 1;
  42 Sign_My = 1;
  43 Sign_Mz = 1;
  44
  45 BUFFER_SIZE = 1095;
  46
  47 Accel_data = zeros(4,BUFFER_SIZE);
  48 Mag_data =  zeros(4,BUFFER_SIZE);
  49
  50 Geo_RV = zeros(4,BUFFER_SIZE);
  51 Orientation_RV = zeros(3,BUFFER_SIZE);
  52
  53 % Sensor Data simulating orientation motions
  54
  55 % get accel x,y,z axis data from stored file
  56 Accel_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,1))') - Bias_Ax)(1:BUFFER_SIZE);
  57 Accel_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,2))') - Bias_Ay)(1:BUFFER_SIZE);
  58 Accel_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,3))') - Bias_Az)(1:BUFFER_SIZE);
  59 Accel_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,4))')(1:BUFFER_SIZE);
  60
  61 % get magnetometer x,y,z axis data from stored file
  62 Mag_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx;
  63 Mag_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My;
  64 Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz;
  65 Mag_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,4))')(1:BUFFER_SIZE);
  66
  67 % estimate orientation
  68 Geo_RV = estimate_geomagnetic_rv(Accel_data, Mag_data);
  69
  70 for i = 1:BUFFER_SIZE
  71         Orientation_RV(:,i) = quat2euler(Geo_RV(i,:));
  72 end
  73
  74 hfig=(figure);
  75 scrsz = get(0,'ScreenSize');
  76 set(hfig,'position',scrsz);
  77 % Geomagnetic Rotation Vector Plot Results
  78 subplot(3,1,1)
  79 UA = Orientation_RV(1,:);
  80 p1 = plot(1:length(UA),UA(1,1:length(UA)),'k');
  81 legend(p1,'x-axis');
  82 title(['Pitch']);
  83 subplot(3,1,2)
  84 UA = Orientation_RV(2,:);
  85 p1 = plot(1:length(UA),UA(1,1:length(UA)),'b');
  86 legend(p1,'y-axis');
  87 title(['Roll']);
  88 subplot(3,1,3)
  89 UA = Orientation_RV(3,:);
  90 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');
  91 legend(p1,'z-axis');
  92 title(['Yaw']);
  93