回答編集履歴
11
コメントに合わせて修正
answer
CHANGED
@@ -1,26 +1,36 @@
|
|
1
1
|
質問文の観測のヤコビ行列は整理すると
|
2
2
|
|
3
|
+
|
3
4
|
```scilab
|
4
5
|
dx = LM(1)-x(1)
|
5
6
|
dy = LM(2)-x(2)
|
7
|
+
r=(dx^2 + dy^2)^0.5;
|
6
|
-
r2=dx^2 + dy^2;
|
8
|
+
r2=(dx^2 + dy^2);
|
7
|
-
Ht = [
|
9
|
+
Ht = [
|
8
|
-
-
|
10
|
+
-dx/r, -dy/r
|
9
|
-
-
|
11
|
+
-dy/r2, -dx/r2;
|
12
|
+
];
|
10
13
|
```
|
11
14
|
|
12
15
|
となっていますが、
|
16
|
+
Ht(1,2) = ∂φ/∂xの計算結果だけマイナスが余計で
|
17
|
+
https://www.wolframalpha.com/input/?i=%28d%2Fdx%29arctan%28%28b-y%29%2F%28a-x%29%29
|
18
|
+
|
13
19
|
正しくは
|
14
20
|
|
15
21
|
```scilab
|
16
22
|
dx = LM(1)-x(1)
|
17
23
|
dy = LM(2)-x(2)
|
24
|
+
r=(dx^2 + dy^2)^0.5;
|
18
|
-
r2=dx^2 + dy^2;
|
25
|
+
r2=(dx^2 + dy^2);
|
19
|
-
Ht = [
|
26
|
+
Ht = [
|
20
|
-
-
|
27
|
+
-dx/r, -dy/r
|
21
|
-
|
28
|
+
dy/r2, -dx/r2;
|
29
|
+
];
|
22
30
|
```
|
23
31
|
|
24
32
|
ではないでしょうか
|
25
33
|
|
34
|
+
---
|
35
|
+
|
26
36
|
[参考](http://www.namerikawa.sd.keio.ac.jp/publications/domestic_conf/2009/movic2009yama.pdf)
|
10
typo
answer
CHANGED
@@ -6,7 +6,7 @@
|
|
6
6
|
r2=dx^2 + dy^2;
|
7
7
|
Ht = [
|
8
8
|
-dy/(r2)^0.5, -dx/(r2)^0.5;
|
9
|
-
-
|
9
|
+
-dx/(r2), -dy/(r2)];
|
10
10
|
```
|
11
11
|
|
12
12
|
となっていますが、
|
@@ -18,7 +18,7 @@
|
|
18
18
|
r2=dx^2 + dy^2;
|
19
19
|
Ht = [
|
20
20
|
-dy/(r2)^0.5, dx/(r2)^0.5;
|
21
|
-
|
21
|
+
dx/(r2), dy/(r2)];
|
22
22
|
```
|
23
23
|
|
24
24
|
ではないでしょうか
|
9
全然見当違いのこと語ってたっぽいので抹消
answer
CHANGED
@@ -1,178 +1,26 @@
|
|
1
|
-
|
1
|
+
質問文の観測のヤコビ行列は整理すると
|
2
2
|
|
3
|
-
* Ftを単位行列に変更(入力込みのヤコビ行列の検算が面倒だったため、とりあえず動かないモデルを採用)
|
4
|
-
* ~~観測関数はLMから見たロボットの位置になっているのに観測行列はロボットから見たLMの位置になっていたのを修正;(LM-x)に統一~~元からそうでした
|
5
|
-
* 観測誤差も[-π, π)に収めるように変更
|
6
|
-
* 観測関数で角度を出す際にノイズを加えてから[-π, π)に収めるように変更
|
7
|
-
|
8
3
|
```scilab
|
4
|
+
dx = LM(1)-x(1)
|
5
|
+
dy = LM(2)-x(2)
|
6
|
+
r2=dx^2 + dy^2;
|
7
|
+
Ht = [
|
9
|
-
|
8
|
+
-dy/(r2)^0.5, -dx/(r2)^0.5;
|
9
|
+
-dy/(r2), -dx/(r2)];
|
10
|
+
```
|
10
11
|
|
11
|
-
|
12
|
-
|
12
|
+
となっていますが、
|
13
|
-
xg = [25;20];
|
14
|
-
global cg
|
15
|
-
cg = 300;
|
16
|
-
global lg
|
17
|
-
lg = 100;
|
18
|
-
global a
|
19
|
-
|
13
|
+
正しくは
|
20
|
-
x0=[0;0];
|
21
|
-
|
22
|
-
global LM
|
23
|
-
LM= [0;10];
|
24
|
-
|
25
|
-
|
26
|
-
|
27
|
-
|
28
|
-
|
29
|
-
result.xTrue=[];
|
30
|
-
result.xd=[];
|
31
|
-
result.xEst=[];
|
32
|
-
result.z=[];
|
33
|
-
result.PEst=[];
|
34
|
-
result.u=[];
|
35
|
-
|
36
|
-
Xact=[0;0];
|
37
|
-
Xdes=[0;0];
|
38
|
-
Xest=[0;0];
|
39
|
-
global z
|
40
|
-
z = [0;0];
|
41
|
-
|
42
|
-
global F
|
43
|
-
F=eye(2,2);
|
44
|
-
H=eye(2,2);
|
45
|
-
Pest = zeros(2,2);
|
46
|
-
|
47
|
-
w=[0.01;0.02];
|
48
|
-
// v=[1;1];
|
49
|
-
v=[0.03;toRadian(1)];
|
50
|
-
// w=[0;0]
|
51
|
-
// v=[0;0]
|
52
|
-
Q=diag(w).^2;
|
53
|
-
R=diag(v).^2;
|
54
|
-
xes=zeros(300,2);
|
55
|
-
xas=zeros(300,2);
|
56
|
-
zps=zeros(300,2);
|
57
|
-
zas=zeros(300,2);
|
58
|
-
ys=zeros(300,2);
|
59
|
-
|
60
|
-
vas=zeros(300,2);
|
61
|
-
ves =zeros(300,2);
|
62
|
-
htxs =zeros(300,2);
|
63
|
-
for t=1:300 do
|
64
|
-
// printf("%d\n",t)
|
65
|
-
Vact = VerocityVectorIncludeNoise(Xact,w);
|
66
|
-
|
67
|
-
Xact = f(Xact,Vact);
|
68
|
-
|
69
|
-
|
70
|
-
Vest = VerocityVectorIncludeNoise(Xest,w*0);
|
71
|
-
Xpred = f(Xest,Vest);
|
72
|
-
[vxdx, vxdy, vydx, vydy] = bibun(Vest,Xest,xg);
|
73
|
-
Ft = jacobF(vxdx,vxdy,vydx,vydy);
|
74
|
-
Ppred = Ft*Pest*Ft' + Q;
|
75
|
-
|
76
|
-
|
77
|
-
z = Observation(Xact,LM,v);
|
78
|
-
zpred = Observation(Xpred,LM,v*0);
|
79
|
-
|
80
|
-
|
81
|
-
y = z - zpred;
|
82
|
-
y(2) = PI2PI(y(2));
|
83
|
-
Ht = JacobianH(Xpred,LM);
|
84
|
-
S = R + Ht*Ppred*Ht';
|
85
|
-
K = Ppred*Ht'/S;
|
86
|
-
|
87
14
|
|
88
|
-
Xest = Xpred + K*y;
|
89
|
-
Pest = (eye(2,2) - K*Ht)*Ppred;
|
90
|
-
|
91
|
-
xes(t,:) = Xest;
|
92
|
-
xas(t,:) = Xact;
|
93
|
-
zps(t,:) = zpred;
|
94
|
-
htxs(t,:) = Ht*Xact;
|
95
|
-
zes(t,:) = z;
|
96
|
-
ys(t,:) = y;
|
97
|
-
|
98
|
-
|
99
|
-
ves(t,:) = Vest;
|
100
|
-
vas(t,:) = Vact;
|
101
|
-
end
|
102
|
-
|
103
|
-
endfunction
|
104
|
-
|
105
|
-
|
106
|
-
|
107
|
-
|
108
|
-
|
109
|
-
|
110
|
-
function V = VerocityVectorIncludeNoise(x,w)
|
111
|
-
global xg;
|
112
|
-
global cg;
|
113
|
-
|
15
|
+
```scilab
|
114
|
-
Vx = -(2*cg*(x(1)-xg(1))/lg^2)*exp(-(((x(1)-xg(1))^2)+(x(2)-xg(2))^2)/lg^2)+w(1)*rand(1,"normal");
|
115
|
-
Vy = -(2*cg*(x(2)-xg(2))/lg^2)*exp(-(((x(2)-xg(2))^2)+(x(2)-xg(2))^2)/lg^2)+w(2)*rand(1,"normal");
|
116
|
-
|
16
|
+
dx = LM(1)-x(1)
|
117
|
-
// V=xg/100 + w*rand(2,"normal");
|
118
|
-
endfunction
|
119
|
-
|
120
|
-
|
17
|
+
dy = LM(2)-x(2)
|
121
|
-
global a;
|
122
|
-
|
18
|
+
r2=dx^2 + dy^2;
|
123
|
-
// x1 = x + v;
|
124
|
-
endfunction
|
125
|
-
|
126
|
-
function z = Observation(x,xm,v)
|
127
|
-
// z(1) = x(1)-xm(1) + v(1)*rand(1,"normal");
|
128
|
-
// z(2) = x(2)-xm(2) + v(2)*rand(1,"normal");
|
129
|
-
z(1) = ((xm(1)-x(1))^2+(xm(2)-x(2))^2)^0.5 + v(1)*rand(1,"normal");
|
130
|
-
z(2) = PI2PI(atan(xm(2)-x(2),xm(1)-x(1))+ v(2)*rand(1,"normal") );
|
131
|
-
endfunction
|
132
|
-
|
133
|
-
function angle=PI2PI(angle)
|
134
|
-
angle=atan(sin(angle),cos(angle));
|
135
|
-
endfunction
|
136
|
-
|
137
|
-
function Ft = jacobF(vxdx,vxdy,vydx,vydy)
|
138
|
-
global F;
|
139
|
-
global a;
|
140
|
-
Ft=eye(2,2)//F+a*[vxdx vxdy;vydx vydy];
|
141
|
-
endfunction
|
142
|
-
|
143
|
-
function Ht = JacobianH(x,LM)
|
144
|
-
// Ht=eye(2,2);
|
145
|
-
r2=(LM(1)-x(1))^2 + (LM(2)-x(2))^2;
|
146
|
-
|
19
|
+
Ht = [
|
147
|
-
|
148
|
-
|
20
|
+
-dy/(r2)^0.5, dx/(r2)^0.5;
|
149
|
-
|
21
|
+
dy/(r2), dx/(r2)];
|
150
|
-
endfunction
|
151
|
-
|
152
|
-
function [vxdx,vxdy,vydx,vydy] = bibun(v,x,xg)
|
153
|
-
global a;
|
154
|
-
global cg;
|
155
|
-
global lg;
|
156
|
-
vxdx = -((2*cg*a)/lg^2)*(exp(-(((x(1)-xg(1))^2)+(x(2)-xg(2))^2)/lg^2))*(1+((x(1)-xg(1))*v(1))/2*a*cg);
|
157
|
-
vxdy = -v(1)*(2*(x(2)-xg(2))/lg^2);
|
158
|
-
vydx = -v(2)*(2*(x(1)-xg(1))/lg^2);
|
159
|
-
vydy = -((2*cg*a)/lg^2)*(exp(-(((x(1)-xg(1))^2)+(x(2)-xg(2))^2)/lg^2))*(1+((x(2)-xg(2))*v(2))/2*a*cg);
|
160
|
-
endfunction
|
161
|
-
|
162
|
-
function radian = toRadian(degree)
|
163
|
-
radian = (degree/180)*%pi;
|
164
|
-
endfunction
|
165
|
-
|
166
22
|
```
|
167
23
|
|
168
|
-
|
24
|
+
ではないでしょうか
|
169
25
|
|
170
|
-
そもそも"角度"ってこういう場面では扱いが面倒です。
|
171
|
-
というのも例えば[-π, π)に収めるとしたら
|
172
|
-
π付近の角度を状態に持ってたときってちょっと状態が変化しただけでも値としては2π近く飛んで
|
173
|
-
いろいろと計算がややこしくなるんですよね。
|
174
|
-
|
175
|
-
(
|
26
|
+
[参考](http://www.namerikawa.sd.keio.ac.jp/publications/domestic_conf/2009/movic2009yama.pdf)
|
176
|
-
|
177
|
-
だから実際にセンサで得られる観測がランドマーク基準の極座標(r, θ)であっても、
|
178
|
-
直交座標に変換したものを観測としたほうが計算は楽です。
|
8
書き忘れ変更点追記
answer
CHANGED
@@ -2,6 +2,8 @@
|
|
2
2
|
|
3
3
|
* Ftを単位行列に変更(入力込みのヤコビ行列の検算が面倒だったため、とりあえず動かないモデルを採用)
|
4
4
|
* ~~観測関数はLMから見たロボットの位置になっているのに観測行列はロボットから見たLMの位置になっていたのを修正;(LM-x)に統一~~元からそうでした
|
5
|
+
* 観測誤差も[-π, π)に収めるように変更
|
6
|
+
* 観測関数で角度を出す際にノイズを加えてから[-π, π)に収めるように変更
|
5
7
|
|
6
8
|
```scilab
|
7
9
|
function [] = VectorFieldKalmanFilterLocalization()
|
7
修正
answer
CHANGED
@@ -1,7 +1,7 @@
|
|
1
1
|

|
2
2
|
|
3
3
|
* Ftを単位行列に変更(入力込みのヤコビ行列の検算が面倒だったため、とりあえず動かないモデルを採用)
|
4
|
-
* 観測関数はLMから見たロボットの位置になっているのに観測行列はロボットから見たLMの位置になっていたのを修正;(LM-x)に統一
|
4
|
+
* ~~観測関数はLMから見たロボットの位置になっているのに観測行列はロボットから見たLMの位置になっていたのを修正;(LM-x)に統一~~元からそうでした
|
5
5
|
|
6
6
|
```scilab
|
7
7
|
function [] = VectorFieldKalmanFilterLocalization()
|
6
typo
answer
CHANGED
@@ -1,7 +1,7 @@
|
|
1
1
|

|
2
2
|
|
3
3
|
* Ftを単位行列に変更(入力込みのヤコビ行列の検算が面倒だったため、とりあえず動かないモデルを採用)
|
4
|
-
* 観測関数はLMから見たロボットの位置になっているのに観測行列はロボットから見たLMの位置になっていたのを修正(LM-x)に統一
|
4
|
+
* 観測関数はLMから見たロボットの位置になっているのに観測行列はロボットから見たLMの位置になっていたのを修正;(LM-x)に統一
|
5
5
|
|
6
6
|
```scilab
|
7
7
|
function [] = VectorFieldKalmanFilterLocalization()
|
5
追記
answer
CHANGED
@@ -1,6 +1,6 @@
|
|
1
1
|

|
2
2
|
|
3
|
-
* Ftを単位行列に変更
|
3
|
+
* Ftを単位行列に変更(入力込みのヤコビ行列の検算が面倒だったため、とりあえず動かないモデルを採用)
|
4
4
|
* 観測関数はLMから見たロボットの位置になっているのに観測行列はロボットから見たLMの位置になっていたのを修正(LM-x)に統一
|
5
5
|
|
6
6
|
```scilab
|
4
修正点
answer
CHANGED
@@ -1,5 +1,7 @@
|
|
1
1
|

|
2
2
|
|
3
|
+
* Ftを単位行列に変更
|
4
|
+
* 観測関数はLMから見たロボットの位置になっているのに観測行列はロボットから見たLMの位置になっていたのを修正(LM-x)に統一
|
3
5
|
|
4
6
|
```scilab
|
5
7
|
function [] = VectorFieldKalmanFilterLocalization()
|
3
暫定
answer
CHANGED
@@ -1,7 +1,5 @@
|
|
1
1
|

|
2
2
|
|
3
|
-
* Htのミス修正(Ht(2,2)成分の^2の位置がおかしかった)
|
4
|
-
* Ftを単位行列に変更
|
5
3
|
|
6
4
|
```scilab
|
7
5
|
function [] = VectorFieldKalmanFilterLocalization()
|
2
回答追加
answer
CHANGED
@@ -1,4 +1,170 @@
|
|
1
|
+

|
2
|
+
|
3
|
+
* Htのミス修正(Ht(2,2)成分の^2の位置がおかしかった)
|
1
|
-
|
4
|
+
* Ftを単位行列に変更
|
5
|
+
|
6
|
+
```scilab
|
7
|
+
function [] = VectorFieldKalmanFilterLocalization()
|
8
|
+
|
9
|
+
|
10
|
+
global xg
|
11
|
+
xg = [25;20];
|
12
|
+
global cg
|
13
|
+
cg = 300;
|
14
|
+
global lg
|
15
|
+
lg = 100;
|
16
|
+
global a
|
17
|
+
a= 0.1;
|
18
|
+
x0=[0;0];
|
19
|
+
|
20
|
+
global LM
|
21
|
+
LM= [0;10];
|
22
|
+
|
23
|
+
|
24
|
+
|
25
|
+
|
26
|
+
|
27
|
+
result.xTrue=[];
|
28
|
+
result.xd=[];
|
29
|
+
result.xEst=[];
|
30
|
+
result.z=[];
|
31
|
+
result.PEst=[];
|
32
|
+
result.u=[];
|
33
|
+
|
34
|
+
Xact=[0;0];
|
35
|
+
Xdes=[0;0];
|
36
|
+
Xest=[0;0];
|
37
|
+
global z
|
38
|
+
z = [0;0];
|
39
|
+
|
40
|
+
global F
|
41
|
+
F=eye(2,2);
|
42
|
+
H=eye(2,2);
|
43
|
+
Pest = zeros(2,2);
|
44
|
+
|
45
|
+
w=[0.01;0.02];
|
46
|
+
// v=[1;1];
|
47
|
+
v=[0.03;toRadian(1)];
|
48
|
+
// w=[0;0]
|
49
|
+
// v=[0;0]
|
50
|
+
Q=diag(w).^2;
|
51
|
+
R=diag(v).^2;
|
52
|
+
xes=zeros(300,2);
|
53
|
+
xas=zeros(300,2);
|
54
|
+
zps=zeros(300,2);
|
55
|
+
zas=zeros(300,2);
|
56
|
+
ys=zeros(300,2);
|
57
|
+
|
58
|
+
vas=zeros(300,2);
|
59
|
+
ves =zeros(300,2);
|
60
|
+
htxs =zeros(300,2);
|
61
|
+
for t=1:300 do
|
62
|
+
// printf("%d\n",t)
|
63
|
+
Vact = VerocityVectorIncludeNoise(Xact,w);
|
64
|
+
|
65
|
+
Xact = f(Xact,Vact);
|
66
|
+
|
67
|
+
|
68
|
+
Vest = VerocityVectorIncludeNoise(Xest,w*0);
|
69
|
+
Xpred = f(Xest,Vest);
|
70
|
+
[vxdx, vxdy, vydx, vydy] = bibun(Vest,Xest,xg);
|
71
|
+
Ft = jacobF(vxdx,vxdy,vydx,vydy);
|
72
|
+
Ppred = Ft*Pest*Ft' + Q;
|
73
|
+
|
74
|
+
|
75
|
+
z = Observation(Xact,LM,v);
|
76
|
+
zpred = Observation(Xpred,LM,v*0);
|
77
|
+
|
78
|
+
|
79
|
+
y = z - zpred;
|
80
|
+
y(2) = PI2PI(y(2));
|
81
|
+
Ht = JacobianH(Xpred,LM);
|
82
|
+
S = R + Ht*Ppred*Ht';
|
83
|
+
K = Ppred*Ht'/S;
|
84
|
+
|
85
|
+
|
86
|
+
Xest = Xpred + K*y;
|
87
|
+
Pest = (eye(2,2) - K*Ht)*Ppred;
|
88
|
+
|
89
|
+
xes(t,:) = Xest;
|
90
|
+
xas(t,:) = Xact;
|
91
|
+
zps(t,:) = zpred;
|
92
|
+
htxs(t,:) = Ht*Xact;
|
93
|
+
zes(t,:) = z;
|
94
|
+
ys(t,:) = y;
|
95
|
+
|
96
|
+
|
97
|
+
ves(t,:) = Vest;
|
98
|
+
vas(t,:) = Vact;
|
99
|
+
end
|
100
|
+
|
101
|
+
endfunction
|
102
|
+
|
103
|
+
|
104
|
+
|
105
|
+
|
106
|
+
|
107
|
+
|
108
|
+
function V = VerocityVectorIncludeNoise(x,w)
|
109
|
+
global xg;
|
110
|
+
global cg;
|
111
|
+
global lg;
|
112
|
+
Vx = -(2*cg*(x(1)-xg(1))/lg^2)*exp(-(((x(1)-xg(1))^2)+(x(2)-xg(2))^2)/lg^2)+w(1)*rand(1,"normal");
|
113
|
+
Vy = -(2*cg*(x(2)-xg(2))/lg^2)*exp(-(((x(2)-xg(2))^2)+(x(2)-xg(2))^2)/lg^2)+w(2)*rand(1,"normal");
|
114
|
+
V = [Vx;Vy];
|
115
|
+
// V=xg/100 + w*rand(2,"normal");
|
116
|
+
endfunction
|
117
|
+
|
118
|
+
function x1 = f(x,v)
|
119
|
+
global a;
|
120
|
+
x1 = x + v*a;
|
121
|
+
// x1 = x + v;
|
122
|
+
endfunction
|
123
|
+
|
124
|
+
function z = Observation(x,xm,v)
|
125
|
+
// z(1) = x(1)-xm(1) + v(1)*rand(1,"normal");
|
126
|
+
// z(2) = x(2)-xm(2) + v(2)*rand(1,"normal");
|
127
|
+
z(1) = ((xm(1)-x(1))^2+(xm(2)-x(2))^2)^0.5 + v(1)*rand(1,"normal");
|
128
|
+
z(2) = PI2PI(atan(xm(2)-x(2),xm(1)-x(1))+ v(2)*rand(1,"normal") );
|
129
|
+
endfunction
|
130
|
+
|
131
|
+
function angle=PI2PI(angle)
|
132
|
+
angle=atan(sin(angle),cos(angle));
|
133
|
+
endfunction
|
134
|
+
|
135
|
+
function Ft = jacobF(vxdx,vxdy,vydx,vydy)
|
136
|
+
global F;
|
137
|
+
global a;
|
138
|
+
Ft=eye(2,2)//F+a*[vxdx vxdy;vydx vydy];
|
139
|
+
endfunction
|
140
|
+
|
141
|
+
function Ht = JacobianH(x,LM)
|
142
|
+
// Ht=eye(2,2);
|
143
|
+
r2=(LM(1)-x(1))^2 + (LM(2)-x(2))^2;
|
144
|
+
Ht = [
|
145
|
+
|
146
|
+
-(LM(1)-x(1))/(r2)^0.5, -(LM(2)-x(2))/(r2)^0.5;
|
147
|
+
-(LM(2)-x(2))/(r2), -(LM(1)-x(1))/(r2)];
|
148
|
+
endfunction
|
149
|
+
|
150
|
+
function [vxdx,vxdy,vydx,vydy] = bibun(v,x,xg)
|
151
|
+
global a;
|
152
|
+
global cg;
|
153
|
+
global lg;
|
154
|
+
vxdx = -((2*cg*a)/lg^2)*(exp(-(((x(1)-xg(1))^2)+(x(2)-xg(2))^2)/lg^2))*(1+((x(1)-xg(1))*v(1))/2*a*cg);
|
155
|
+
vxdy = -v(1)*(2*(x(2)-xg(2))/lg^2);
|
156
|
+
vydx = -v(2)*(2*(x(1)-xg(1))/lg^2);
|
157
|
+
vydy = -((2*cg*a)/lg^2)*(exp(-(((x(1)-xg(1))^2)+(x(2)-xg(2))^2)/lg^2))*(1+((x(2)-xg(2))*v(2))/2*a*cg);
|
158
|
+
endfunction
|
159
|
+
|
160
|
+
function radian = toRadian(degree)
|
161
|
+
radian = (degree/180)*%pi;
|
162
|
+
endfunction
|
163
|
+
|
164
|
+
```
|
165
|
+
|
166
|
+
---
|
167
|
+
|
2
168
|
そもそも"角度"ってこういう場面では扱いが面倒です。
|
3
169
|
というのも例えば[-π, π)に収めるとしたら
|
4
170
|
π付近の角度を状態に持ってたときってちょっと状態が変化しただけでも値としては2π近く飛んで
|
1
typo
answer
CHANGED
@@ -6,5 +6,5 @@
|
|
6
6
|
|
7
7
|
(3次元の姿勢推定は微小変化でも値がホイホイ飛ぶオイラー角より連続的に変化するクォータニオンの方を採用することが多いわけで。)
|
8
8
|
|
9
|
-
だから実際にセンサで得られる観測がランドマーク基準の極座標(r, θ)
|
9
|
+
だから実際にセンサで得られる観測がランドマーク基準の極座標(r, θ)であっても、
|
10
10
|
直交座標に変換したものを観測としたほうが計算は楽です。
|