Modifying sf_orientation for new estimate_orientation
[platform/core/system/sensord.git] / src / sensor_fusion / 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_Mx = 1;
52 Sign_My = 1;
53 Sign_Mz = 1;
54
55 BUFFER_SIZE = 1095;
56
57 Accel_data = zeros(4,BUFFER_SIZE);
58 Gyro_data = zeros(4,BUFFER_SIZE);
59 Mag_data =  zeros(4,BUFFER_SIZE);
60
61 OR_driv = zeros(3,BUFFER_SIZE);
62 OR_aid = zeros(3,BUFFER_SIZE);
63 OR_err = zeros(3,BUFFER_SIZE);
64
65 euler_driv = zeros(BUFFER_SIZE,3);
66 euler_aid = zeros(BUFFER_SIZE,3);
67 euler_err = zeros(BUFFER_SIZE,3);
68
69 % Sensor Data simulating orientation motions
70
71 % get accel x,y,z axis data from stored file
72 Accel_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,1))') - Bias_Ax)(1:BUFFER_SIZE);
73 Accel_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,2))') - Bias_Ay)(1:BUFFER_SIZE);
74 Accel_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,3))') - Bias_Az)(1:BUFFER_SIZE);
75 Accel_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/accel.txt")(:,4))')(1:BUFFER_SIZE);
76
77 % get gyro x,y,z axis data from stored file
78 Gyro_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,1))') - Bias_Gx)(1:BUFFER_SIZE);
79 Gyro_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,2))') - Bias_Gy)(1:BUFFER_SIZE);
80 Gyro_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,3))') - Bias_Gz)(1:BUFFER_SIZE);
81 Gyro_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,4))')(1:BUFFER_SIZE);
82
83 scale_Gyro = 575;
84 Gyro_data(1,:) = Gyro_data(1,:)/scale_Gyro;
85 Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro;
86 Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro;
87
88 % get magnetometer x,y,z axis data from stored file
89 Mag_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx;
90 Mag_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My;
91 Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz;
92 Mag_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,4))')(1:BUFFER_SIZE);
93
94 % estimate orientation
95 [Quat_driv, Quat_aid, Quat_err]  = estimate_orientation(Accel_data, Gyro_data, Mag_data);
96
97 for i = 1:BUFFER_SIZE
98         euler_aid(i,:) = quat2euler(Quat_aid(i,:));
99         OR_aid(1,i) = euler_aid(i,2)' * RAD2DEG;
100         OR_aid(2,i) = euler_aid(i,1)' * RAD2DEG;
101         OR_aid(3,i) = euler_aid(i,3)' * RAD2DEG;
102
103         euler_driv(i,:) = quat2euler(Quat_driv(i,:));
104         OR_driv(1,i) = ROLL_PHASE_CORRECTION * euler_driv(i,2)' * RAD2DEG;
105         OR_driv(2,i) = PITCH_PHASE_CORRECTION * euler_driv(i,1)' * RAD2DEG;
106         OR_driv(3,i) = YAW_PHASE_CORRECTION * euler_driv(i,3)' * RAD2DEG;
107
108         euler_err(i,:) = quat2euler(Quat_err(i,:));
109         OR_err(1,i) = euler_err(i,2)' * RAD2DEG;
110         OR_err(2,i) = euler_err(i,1)' * RAD2DEG;
111         OR_err(3,i) = euler_err(i,3)' * RAD2DEG;
112 end
113
114 % Rotation Plot Results
115 hfig=(figure);
116 scrsz = get(0,'ScreenSize');
117 set(hfig,'position',scrsz);
118 subplot(3,1,1)
119 UA = OR_aid(2,:);
120 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');
121 hold on;
122 grid on;
123 UA = OR_driv(2,:);
124 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
125 hold on;
126 grid on;
127 UA = OR_err(2,:);
128 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');
129 title(['Pitch']);
130 legend([p1 p2 p3],'Aiding System', 'Driving System', 'Quaternion based error');
131 subplot(3,1,2)
132 UA = OR_aid(1,:);
133 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');
134 hold on;
135 grid on;
136 UA = OR_driv(1,:);
137 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
138 hold on;
139 grid on;
140 UA = OR_err(1,:);
141 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');
142 title(['Roll']);
143 legend([p1 p2 p3],'Aiding System', 'Driving System', 'Quaternion based error');
144 subplot(3,1,3)
145 UA = OR_aid(3,:);
146 p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');
147 hold on;
148 grid on;
149 UA = OR_driv(3,:);
150 p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
151 hold on;
152 grid on;
153 UA = OR_err(3,:);
154 p3 = plot(1:length(UA),UA(1,1:length(UA)),'g');
155 title(['Yaw']);
156 legend([p1 p2 p3],'Aiding System', 'Driving System', 'Quaternion based error');