src/fusion-sensor/rotation_vector/design/sf_orientation.m

   1 % sf_orientation
   2 %
   3 % Copyright (c) 2014 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 Orientation Estimation
  18 %
  19 % - Input Accelerometer, Gyroscope and Magnetometer sensor data
  20 % - Call estimate_orientation function
  21 % - Plot results for orientation
  22
  23 addpath('lib');
  24 clear
  25 close all
  26 clc
  27
  28 GRAVITY = 9.80665;
  29 RAD2DEG = 57.2957795;
  30
  31 Max_Range_Accel = 39.203407; Min_Range_Accel = -39.204006; Res_Accel = 0.000598;
  32 Max_Range_Gyro = 1146.862549; Min_Range_Gyro = -1146.880005; Res_Gyro = 0.017500;
  33 Max_Range_Magnetic = 1200; Min_Range_Magnetic = -1200; Res_Magnetic = 1;
  34
  35 PITCH_PHASE_CORRECTION = -1;
  36 ROLL_PHASE_CORRECTION = -1;
  37 YAW_PHASE_CORRECTION = -1;
  38
  39 Bias_Ax = 0.098586;
  40 Bias_Ay = 0.18385;
  41 Bias_Az = 10.084 - GRAVITY;
  42
  43 Bias_Gx = -5.3539;
  44 Bias_Gy = 0.24325;
  45 Bias_Gz = 2.3391;
  46
  47 Bias_Mx = 0;
  48 Bias_My = 0;
  49 Bias_Mz = 0;
  50
  51 Sign_Ax = 1;
  52 Sign_Ay = 1;
  53 Sign_Az = 1;
  54
  55 Sign_Gx = 1;
  56 Sign_Gy = 1;
  57 Sign_Gz = 1;
  58
  59 Sign_Mx = 1;
  60 Sign_My = 1;
  61 Sign_Mz = 1;
  62
  63 BUFFER_SIZE = 1095;
  64
  65 Accel_data = zeros(4,BUFFER_SIZE);
  66 Gyro_data = zeros(4,BUFFER_SIZE);
  67 Mag_data =  zeros(4,BUFFER_SIZE);
  68
  69 OR_driv = zeros(3,BUFFER_SIZE);
  70 OR_aid = zeros(3,BUFFER_SIZE);
  71 OR_err = zeros(3,BUFFER_SIZE);
  72
  73 euler_driv = zeros(BUFFER_SIZE,3);
  74 euler_aid = zeros(BUFFER_SIZE,3);
  75 euler_err = zeros(BUFFER_SIZE,3);
  76
  77 % Sensor Data simulating orientation motions
  78
  79 % get accel x,y,z axis data from stored file
  80 Accel_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,1))') - Bias_Ax)(1:BUFFER_SIZE);
  81 Accel_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,2))') - Bias_Ay)(1:BUFFER_SIZE);
  82 Accel_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,3))') - Bias_Az)(1:BUFFER_SIZE);
  83 Accel_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,4))')(1:BUFFER_SIZE);
  84
  85 % get gyro x,y,z axis data from stored file
  86 Gyro_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,1))') - Bias_Gx)(1:BUFFER_SIZE);
  87 Gyro_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,2))') - Bias_Gy)(1:BUFFER_SIZE);
  88 Gyro_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,3))') - Bias_Gz)(1:BUFFER_SIZE);
  89 Gyro_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,4))')(1:BUFFER_SIZE);
  90
  91 scale_Gyro = 575;
  92 Gyro_data(1,:) = Gyro_data(1,:)/scale_Gyro;
  93 Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro;
  94 Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro;
  95
  96 % get magnetometer x,y,z axis data from stored file
  97 Mag_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,1))') - Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx;
  98 Mag_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,2))') - Bias_My)(1:BUFFER_SIZE) * Sign_My;
  99 Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,3))') - Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz;
 100 Mag_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,4))')(1:BUFFER_SIZE);
 101
 102 % estimate orientation
 103 [Quat_driv, Quat_aid, Quat_err, Gyro_bias]  = estimate_orientation(Accel_data, Gyro_data, Mag_data);
 104
 105 Gyro_bias(1,:) = Gyro_bias(1,:) + Bias_Gx;
 106 Gyro_bias(2,:) = Gyro_bias(2,:) + Bias_Gy;
 107 Gyro_bias(3,:) = Gyro_bias(3,:) + Bias_Gz;
 108
 109 for i = 1:BUFFER_SIZE
 110         euler_aid(i,:) = quat2euler(Quat_aid(i,:));
 111         OR_aid(1,i) = euler_aid(i,2)' * RAD2DEG;
 112         OR_aid(2,i) = euler_aid(i,1)' * RAD2DEG;
 113         OR_aid(3,i) = euler_aid(i,3)' * RAD2DEG;
 114
 115         euler_driv(i,:) = quat2euler(Quat_driv(i,:));
 116         OR_driv(1,i) = ROLL_PHASE_CORRECTION * euler_driv(i,2)' * RAD2DEG;
 117         OR_driv(2,i) = PITCH_PHASE_CORRECTION * euler_driv(i,1)' * RAD2DEG;
 118         OR_driv(3,i) = YAW_PHASE_CORRECTION * euler_driv(i,3)' * RAD2DEG;
 119
 120         if (OR_driv(3,i) < 0)
 121                 OR_driv(3,i) = OR_driv(3,i) + 360;
 122         end
 123
 124         if (OR_aid(3,i) < 0)
 125                 OR_aid(3,i) = OR_aid(3,i) + 360;
 126         end
 127 end
 128
 129 % Rotation Plot Results
 130 hfig=(figure);
 131 scrsz = get(0,'ScreenSize');
 132 set(hfig,'position',scrsz);
 133 subplot(3,1,1)
 134 UA = OR_aid(2,:);
 135 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');
 136 hold on;
 137 grid on;
 138 UA = OR_driv(2,:);
 139 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
 140 hold on;
 141 grid on;
 142 UA = Gyro_bias(1,:);
 143 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');
 144 title(['Pitch']);
 145 legend([p1 p2 p3],'Aiding System', 'Driving System', 'Gyroscope Bias error');
 146 subplot(3,1,2)
 147 UA = OR_aid(1,:);
 148 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');
 149 hold on;
 150 grid on;
 151 UA = OR_driv(1,:);
 152 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
 153 hold on;
 154 grid on;
 155 UA = Gyro_bias(2,:);
 156 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');
 157 title(['Roll']);
 158 legend([p1 p2 p3],'Aiding System', 'Driving System', 'Gyroscope Bias error');
 159 subplot(3,1,3)
 160 UA = OR_aid(3,:);
 161 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');
 162 hold on;
 163 grid on;
 164 UA = OR_driv(3,:);
 165 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
 166 hold on;
 167 grid on;
 168 UA = Gyro_bias(3,:);
 169 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');
 170 title(['Yaw']);
 171 legend([p1 p2 p3],'Aiding System', 'Driving System', 'Gyroscope Bias error');