-
Notifications
You must be signed in to change notification settings - Fork 1
/
LS_mainloop.m
95 lines (62 loc) · 1.69 KB
/
LS_mainloop.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
close all
clear
clc
n = 10; % interation number
load multi_SLAM_pose
load multi_SLAM_obse
X0 = gt(:,1:4) + normrnd(0,0.09, [9,4]);
% X0(:,1) = gt(:,1);
F0 = feature + normrnd(0,0.09,[2,3]);
X = [];
% initialization
for i = 1:size(gt,2)
X = [X;X0(:,i)];
end
for i = 1:size(feature,2)
X = [X;F0(:,i)];
end
for inter = 1:n
Xr1_0=X(1:3);
Ur1 = [1;pi/6];
Xr2_0=X(4:6);
Ur2 = [2;pi/6];
Xr3_0=X(7:9);
Ur3 = [3;pi/6];
Xf1=X(37:38);
Xf2=X(39:40);
Xf3=X(41:42);
for t = 1:3
e_o = X(1:3)-gt();
e_o= X(4:6);
e_o= X(7:9);
ob_r1_f1 = calobservation(gt(1:3,i),Xf1,0); % x,feature,variance
ob_r1_f2 = calobservation(gt(1:3,i),Xf2,0);
ob_r1_f3 = calobservation(gt(1:3,i),Xf3,0);
ob_r2_f2 = calobservation(gt(4:6,i),Xf2,0);
ob_r2_f3 = calobservation(gt(4:6,i),Xf3,0);
ob_r3_f1 = calobservation(gt(7:9,i),Xf1,0);
ob_r3_f3 = calobservation(gt(7:9,i),Xf3,0);
end
O01_X=func_H_O(Xr1,phir1,Xr0,phir0);
O12_X=func_H_O(Xr2,phir2,Xr1,phir1);
Z0f1_X=func_H_Z(Xf1,Xr0,phir0);
Z0f2_X=func_H_Z(Xf2,Xr0,phir0);
Z1f1_X=func_H_Z(Xf1,Xr1,phir1);
Z1f2_X=func_H_Z(Xf2,Xr1,phir1);
Z1f3_X=func_H_Z(Xf3,Xr1,phir1);
Z2f2_X=func_H_Z(Xf2,Xr2,phir2);
Z2f3_X=func_H_Z(Xf3,Xr2,phir2);
eO=[(O01-O01_X);(O12-O12_X)];
eZ=[(Z0f1-Z0f1_X);
(Z0f2-Z0f2_X);
(Z1f1-Z1f1_X);
(Z1f2-Z1f2_X);
(Z1f3-Z1f3_X);
(Z2f2-Z2f2_X);
(Z2f3-Z2f3_X)];
e = [eO;eZ];
% e = eZ;
J = slam_grad(X);
X_new = inv(J'*J)*J'*(e+J*X);
X = X_new
end