src/sensor/rotation_vector/design/sf_linear_acceleration.m

   1 % sf_linear_acceleration
   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 Determination of linear acceleration
  18 %
  19 % - Input Accelerometer, Gyroscope and Magnetometer sensor data
  20 % - Call estimate_linear_acceleration
  21 % - Plot results for linear acceleration on different reference axes
  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_Gyro = 1146.862549; Min_Range_Gyro = -1146.880005; Res_Gyro = 0.017500;
  32 Max_Range_Magnetic = 1200; Min_Range_Magnetic = -1200; Res_Magnetic = 1;
  33
  34 Bias_Ax = 0.098586;
  35 Bias_Ay = 0.18385;
  36 Bias_Az = 10.084 - GRAVITY;
  37
  38 Bias_Gx = -5.3539;
  39 Bias_Gy = 0.24325;
  40 Bias_Gz = 2.3391;
  41
  42 Bias_Mx = 0;
  43 Bias_My = 0;
  44 Bias_Mz = 0;
  45
  46 Sign_Mx = 1;
  47 Sign_My = 1;
  48 Sign_Mz = 1;
  49
  50 BUFFER_SIZE = 125;
  51
  52 Accel_data = zeros(4,BUFFER_SIZE);
  53 Gyro_data = zeros(4,BUFFER_SIZE);
  54 Mag_data =  zeros(4,BUFFER_SIZE);
  55
  56 OR_driv = zeros(3,BUFFER_SIZE);
  57 OR_aid = zeros(3,BUFFER_SIZE);
  58 OR_err = zeros(3,BUFFER_SIZE);
  59
  60 % Sensor Data simulating orientation motions
  61
  62 % get accel x,y,z axis data from stored file
  63 Accel_data(1,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/accel.txt")(:,1))') - Bias_Ax)(1:BUFFER_SIZE);
  64 Accel_data(2,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/accel.txt")(:,2))') - Bias_Ay)(1:BUFFER_SIZE);
  65 Accel_data(3,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/accel.txt")(:,3))') - Bias_Az)(1:BUFFER_SIZE);
  66 Accel_data(4,:) = ((dlmread("data/100ms/linear_acceleration/move_x_y_z/accel.txt")(:,4))')(1:BUFFER_SIZE);
  67
  68 % get gyro x,y,z axis data from stored file
  69 Gyro_data(1,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/gyro.txt")(:,1))') - Bias_Gx)(1:BUFFER_SIZE);
  70 Gyro_data(2,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/gyro.txt")(:,2))') - Bias_Gy)(1:BUFFER_SIZE);
  71 Gyro_data(3,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/gyro.txt")(:,3))') - Bias_Gz)(1:BUFFER_SIZE);
  72 Gyro_data(4,:) = ((dlmread("data/100ms/linear_acceleration/move_x_y_z/gyro.txt")(:,4))')(1:BUFFER_SIZE);
  73
  74 scale_Gyro = 575;
  75 Gyro_data(1,:) = Gyro_data(1,:)/scale_Gyro;
  76 Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro;
  77 Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro;
  78
  79 % get magnetometer x,y,z axis data from stored file
  80 Mag_data(1,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx;
  81 Mag_data(2,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My;
  82 Mag_data(3,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz;
  83 Mag_data(4,:) = ((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,4))')(1:BUFFER_SIZE);
  84
  85 % estimate orientation
  86 Linear_acceleration = estimate_linear_acceleration(Accel_data, Gyro_data, Mag_data);
  87
  88 hfig=(figure);
  89 scrsz = get(0,'ScreenSize');
  90 set(hfig,'position',scrsz);
  91 % Accelerometer Raw data
  92 subplot(2,1,1)
  93 UA = Accel_data(1,:);
  94 p1 = plot(1:length(UA),UA(1,1:length(UA)),'k');
  95 hold on;
  96 grid on;
  97 UA = Accel_data(2,:);
  98 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
  99 hold on;
 100 grid on;
 101 UA = Accel_data(3,:);
 102 p3 = plot(1:length(UA),UA(1,1:length(UA)),'r');
 103 title(['Raw Accelerometer Data']);
 104 legend([p1 p2 p3],'x-axis', 'y-axis', 'z-axis');
 105
 106 % Linear Acceleration Plot Results
 107 subplot(2,1,2)
 108 UA = Linear_acceleration(1,:);
 109 p1 = plot(1:length(UA),UA(1,1:length(UA)),'k');
 110 hold on;
 111 grid on;
 112 UA = Linear_acceleration(2,:);
 113 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
 114 hold on;
 115 grid on;
 116 UA = Linear_acceleration(3,:);
 117 p3 = plot(1:length(UA),UA(1,1:length(UA)),'r');
 118 title(['Linear Acceleration']);
 119 legend([p1 p2 p3],'x-axis', 'y-axis', 'z-axis');