From 731d2efdd906d4fd7870f12991d11cafbe2b26b8 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 6 Jan 2015 15:08:23 +0530 Subject: [PATCH] adding estimate_geomagtic_rv implementation Adding wrapper octave implementation for computing geomagnetic rotation vector which would use the same estimate_orientation implementation for computing the 9-axis rotation vector. Change-Id: Id3f16a34357bc5439276521ac72c368cd7c8d591 --- .../design/lib/estimate_geomagnetic_rv.m | 47 ++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100755 src/sensor_fusion/design/lib/estimate_geomagnetic_rv.m diff --git a/src/sensor_fusion/design/lib/estimate_geomagnetic_rv.m b/src/sensor_fusion/design/lib/estimate_geomagnetic_rv.m new file mode 100755 index 0000000..a3ac4da --- /dev/null +++ b/src/sensor_fusion/design/lib/estimate_geomagnetic_rv.m @@ -0,0 +1,47 @@ +% estimate_gravity +% +% Copyright (c) 2014 Samsung Electronics Co., Ltd. +% +% Licensed under the Apache License, Version 2.0 (the "License"); +% you may not use this file except in compliance with the License. +% You may obtain a copy of the License at +% +% http://www.apache.org/licenses/LICENSE-2.0 +% +% Unless required by applicable law or agreed to in writing, software +% distributed under the License is distributed on an "AS IS" BASIS, +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +% See the License for the specific language governing permissions and +% limitations under the License. + +% Gravitational Force Estimation function +% +% - Orientation Estimation using estimate_orientation function +% - Project the orientation on the device reference axes to obtain +% gravitational force on specific reference axes + + +function [Quat_aid] = estimate_geomagnetic_rv(Accel_data, Mag_data) + +GRAVITY = 9.80665; +RAD2DEG = 57.2957795; + +PITCH_PHASE_CORRECTION = -1; +ROLL_PHASE_CORRECTION = -1; +YAW_PHASE_CORRECTION = -1; + +BUFFER_SIZE = size(Accel_data,2); + +Gravity = zeros(3,BUFFER_SIZE); + +OR_driv = zeros(3,BUFFER_SIZE); +Gyro_data = zeros(3,BUFFER_SIZE); + +Quat_driv = zeros(4,BUFFER_SIZE); +Quat_aid = zeros(4,BUFFER_SIZE); +Quat_err = zeros(4,BUFFER_SIZE); + +% estimate orientation +[Quat_driv, Quat_aid, Quat_err] = estimate_orientation(Accel_data, Gyro_data, Mag_data); + +end -- 2.7.4