src/sensor/sensor_fusion/design/lib/sf_pedometer.m

   1 % sf_pedometer
   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 Pedometer Estimation
  18 %
  19 % - Input Accelerometer, Gyroscope and Magnetometer sensor data
  20 % - Plot results for Pedometer
  21
  22 addpath('lib');
  23 clear
  24 close all
  25 clc
  26
  27 GRAVITY = 9.80665;
  28
  29 Bias_Ax = 0.098586;
  30 Bias_Ay = 0.18385;
  31 Bias_Az = 10.084 - GRAVITY;
  32
  33 Bias_Gx = -5.3539;
  34 Bias_Gy = 0.24325;
  35 Bias_Gz = 2.3391;
  36
  37 Bias_Mx = 0;
  38 Bias_My = 0;
  39 Bias_Mz = 0;
  40
  41 Sign_Ax = 1;
  42 Sign_Ay = 1;
  43 Sign_Az = 1;
  44
  45 Sign_Gx = 1;
  46 Sign_Gy = 1;
  47 Sign_Gz = 1;
  48
  49 Sign_Mx = 1;
  50 Sign_My = 1;
  51 Sign_Mz = 1;
  52
  53 BUFFER_SIZE = 8700;
  54
  55 Accel_data = zeros(4,BUFFER_SIZE);
  56 Gyro_data = zeros(4,BUFFER_SIZE);
  57 Mag_data =  zeros(4,BUFFER_SIZE);
  58
  59 % Sensor Data simulating pedometer motions
  60
  61 % get accel x,y,z axis data from stored file
  62 Accel_data(1,:) = (((dlmread("accel")(:,1))') - Bias_Ax)(1:BUFFER_SIZE);
  63 Accel_data(2,:) = (((dlmread("accel")(:,2))') - Bias_Ay)(1:BUFFER_SIZE);
  64 Accel_data(3,:) = (((dlmread("accel")(:,3))') - Bias_Az)(1:BUFFER_SIZE);
  65 Accel_data(4,:) = ((dlmread("accel")(:,4))')(1:BUFFER_SIZE);
  66
  67 % get gyro x,y,z axis data from stored file
  68 Gyro_data(1,:) = (((dlmread("gyro")(:,1))') - Bias_Gx)(1:BUFFER_SIZE);
  69 Gyro_data(2,:) = (((dlmread("gyro")(:,2))') - Bias_Gy)(1:BUFFER_SIZE);
  70 Gyro_data(3,:) = (((dlmread("gyro")(:,3))') - Bias_Gz)(1:BUFFER_SIZE);
  71 Gyro_data(4,:) = ((dlmread("gyro")(:,4))')(1:BUFFER_SIZE);
  72
  73 scale_Gyro = 1150;
  74 Gyro_data(1,:) = Gyro_data(1,:)/scale_Gyro;
  75 Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro;
  76 Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro;
  77
  78 % get magnetometer x,y,z axis data from stored file
  79 Mag_data(1,:) = (((dlmread("magnetic")(:,1))') - Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx;
  80 Mag_data(2,:) = (((dlmread("magnetic")(:,2))') - Bias_My)(1:BUFFER_SIZE) * Sign_My;
  81 Mag_data(3,:) = (((dlmread("magnetic")(:,3))') - Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz;
  82 Mag_data(4,:) = ((dlmread("magnetic")(:,4))')(1:BUFFER_SIZE);
  83
  84 hfig=(figure);
  85 scrsz = get(0,'ScreenSize');
  86 set(hfig,'position',scrsz);
  87
  88 subplot(3,1,1)
  89 UA = Accel_data(1,:);
  90 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');
  91 hold on;
  92 grid on;
  93 UA = Accel_data(2,:);
  94 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
  95 hold on;
  96 grid on;
  97 UA = Accel_data(3,:);
  98 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');
  99 title(['Accel']);
 100
 101 subplot(3,1,2)
 102 UA = Gyro_data(1,:);
 103 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');
 104 hold on;
 105 grid on;
 106 UA = Gyro_data(2,:);
 107 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
 108 hold on;
 109 grid on;
 110 UA = Gyro_data(3,:);
 111 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');
 112 title(['Gyro']);
 113
 114 subplot(3,1,3)
 115 UA = Mag_data(1,:);
 116 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');
 117 hold on;
 118 grid on;
 119 UA = Mag_data(2,:);
 120 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
 121 hold on;
 122 grid on;
 123 UA = Mag_data(3,:);
 124 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');
 125 title(['Magnetic']);