src/fusion-sensor/rotation_vector/design/lib/estimate_gravity.m

   1 % estimate_gravity
   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 % Gravitational Force Estimation function
  18 %
  19 % - Orientation Estimation using estimate_orientation function
  20 % - Project the orientation on the device reference axes to obtain
  21 %   gravitational force on specific reference axes
  22
  23
  24 function [Gravity]  = estimate_gravity(Accel_data, Gyro_data, Mag_data)
  25
  26 GRAVITY = 9.80665;
  27 RAD2DEG = 57.2957795;
  28
  29 PITCH_PHASE_CORRECTION = -1;
  30 ROLL_PHASE_CORRECTION = -1;
  31 YAW_PHASE_CORRECTION = -1;
  32
  33 BUFFER_SIZE = size(Accel_data,2);
  34
  35 OR_driv = zeros(3,BUFFER_SIZE);
  36
  37 Gravity = zeros(3,BUFFER_SIZE);
  38
  39 OR_driv = zeros(3,BUFFER_SIZE);
  40
  41 Quat_driv = zeros(4,BUFFER_SIZE);
  42 Quat_aid = zeros(4,BUFFER_SIZE);
  43 Quat_err = zeros(4,BUFFER_SIZE);
  44
  45 euler = zeros(BUFFER_SIZE,3);
  46
  47 % estimate orientation
  48 [Quat_driv, Quat_aid, Quat_err]  = estimate_orientation(Accel_data, Gyro_data, Mag_data);
  49
  50 euler = quat2euler(Quat_driv);
  51 OR_driv(1,:) = ROLL_PHASE_CORRECTION * euler(:,2)' * RAD2DEG;
  52 OR_driv(2,:) = PITCH_PHASE_CORRECTION * euler(:,1)' * RAD2DEG;
  53 OR_driv(3,:) = YAW_PHASE_CORRECTION * euler(:,3)' * RAD2DEG;
  54
  55 Gx = GRAVITY * sind(OR_driv(1,:));
  56 Gy = GRAVITY * sind(OR_driv(2,:));
  57 Gz = GRAVITY * cosd(OR_driv(2,:)) .* cosd(OR_driv(1,:));
  58
  59 Gravity = [Gx; Gy; Gz];
  60
  61 end