-
Notifications
You must be signed in to change notification settings - Fork 264
/
Copy pathICRA_2014_gyro.m
115 lines (94 loc) · 3.04 KB
/
ICRA_2014_gyro.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
function [Tg,Kg]=ICRA_2014_gyro(rotation,a0)
% conference: A Robust and Easy to implement method for imu
% calibration without External Equipments
% author Zhang Xin
if nargin<2
a0=[0,0,0,0,0,0,0.001,0.001,0.001];
end
% options=optimset('Algorithm','Levenberg-Marquardt',...
% 'Display','iter','TolX',1e-4,'MaxIter',10);
% options=optimset('Algorithm','Levenberg-Marquardt',...
% 'Display','iter','TolX',1e-5,'MaxIter',18);
% a=lsqnonlin(@rotation_gyro,a0,[],[],options,rotation);
fprintf('Calibration Gyroscope:\n');
TolX=1e-5;
TolFun=0;
MaxIter=18;
a=Optimize_my_LM(@rotation_gyro,a0,rotation,TolX,TolFun,MaxIter);
Tg=[ 1 , -a(1) , a(2) ;...
a(3) , 1 , -a(4) ;...
-a(5) , a(6) , 1];
Kg=[ a(7) , 0 , 0 ;...
0 , a(8) , 0 ;...
0 , 0 , a(9)];
end
function E=rotation_gyro(a,rotation)
Tg=[ 1 , -a(1) , a(2) ;...
a(3) , 1 , -a(4) ;...
-a(5) , a(6) , 1];
Kg=[ a(7) , 0 , 0 ;...
0 , a(8) , 0 ;...
0 , 0 , a(9)];
Ta=rotation{end-3};
Ka=rotation{end-2};
Ba=rotation{end-1};
Bg=rotation{end};
for i=1:size(rotation,1)-4
data=rotation{i};
g_start=Ta*Ka*(data(1,2:4)'+Ba);
g_end=Ta*Ka*(data(end,2:4)'+Ba);
Q(:,1)=[1;0;0;0];
for j=2:size(data,1)
gyro0=Tg*Kg*(data(j-1,5:7)'+Bg);
gyro1=Tg*Kg*(data(j,5:7)'+Bg);
dt=(data(j,1)-data(j-1,1));
Q(:,j)=attitude_update_RK4(Q(:,j-1),dt,gyro0,gyro1);
end
R = quatern2rotMat(Q(:,j)');
g_end_compute=R*g_start;
E((i-1)*3+1,1)=g_end(1)-g_end_compute(1);
E((i-1)*3+2,1)=g_end(2)-g_end_compute(2);
E((i-1)*3+3,1)=g_end(3)-g_end_compute(3);
end
%E(size(rotation,1)-3:size(rotation,1),1)=[0;0;0;0];
end
function [Qk_plus1]=attitude_update_RK4(Qk,dt,gyro0,gyro1)
% RK4
% conference: A Robust and Easy to implement method for imu
% calibration without External Equipments
q_1=Qk;
k1=(1/2)*omegaMatrix(gyro0)*q_1;
q_2=Qk+dt*(1/2)*k1;
k2=(1/2)*omegaMatrix((1/2)*(gyro0+gyro1))*q_2;
q_3=Qk+dt*(1/2)*k2;
k3=(1/2)*omegaMatrix((1/2)*(gyro0+gyro1))*q_3;
q_4=Qk+dt*k3;
k4=(1/2)*omegaMatrix(gyro1)*q_4;
Qk_plus1=Qk+dt*(k1/6+k2/3+k3/3+k4/6);
Qk_plus1=Qk_plus1/norm(Qk_plus1);
end
function [omega]=omegaMatrix(data)
% wx=data(1)*pi/180;
% wy=data(2)*pi/180;
% wz=data(3)*pi/180;
wx=data(1);
wy=data(2);
wz=data(3);
omega=[0 , -wx , -wy , -wz ;...
wx , 0 , wz , -wy ;...
wy , -wz , 0 , wx ;...
wz , wy , -wx , 0 ];
end
function R = quatern2rotMat(q)
[rows cols] = size(q);
R = zeros(3,3, rows);
R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2;
R(1,2,:) = 2.*(q(:,2).*q(:,3)+q(:,1).*q(:,4));
R(1,3,:) = 2.*(q(:,2).*q(:,4)-q(:,1).*q(:,3));
R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4));
R(2,2,:) = 2.*q(:,1).^2-1+2.*q(:,3).^2;
R(2,3,:) = 2.*(q(:,3).*q(:,4)+q(:,1).*q(:,2));
R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3));
R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2));
R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2;
end