teratail header banner
teratail header banner
質問するログイン新規登録

回答編集履歴

11

コメントに合わせて修正

2019/12/05 08:10

投稿

ozwk
ozwk

スコア13553

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
- -dy/(r2)^0.5, -dx/(r2)^0.5;
10
+ -dx/r, -dy/r
9
- -dx/(r2), -dy/(r2)];
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
- -dy/(r2)^0.5, dx/(r2)^0.5;
27
+ -dx/r, -dy/r
21
- dx/(r2), dy/(r2)];
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

2019/12/05 08:09

投稿

ozwk
ozwk

スコア13553

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
- -dy/(r2), -dx/(r2)];
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
- dy/(r2), dx/(r2)];
21
+ dx/(r2), dy/(r2)];
22
22
  ```
23
23
 
24
24
  ではないでしょうか

9

全然見当違いのこと語ってたっぽいので抹消

2019/12/05 05:50

投稿

ozwk
ozwk

スコア13553

answer CHANGED
@@ -1,178 +1,26 @@
1
- ![イメージ説明](2f063477e615a8dba374c347a970ac03.png)
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
- function [] = VectorFieldKalmanFilterLocalization()
8
+ -dy/(r2)^0.5, -dx/(r2)^0.5;
9
+ -dy/(r2), -dx/(r2)];
10
+ ```
10
11
 
11
-
12
- global xg
12
+ となっていますが、
13
- xg = [25;20];
14
- global cg
15
- cg = 300;
16
- global lg
17
- lg = 100;
18
- global a
19
- a= 0.1;
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
- global lg;
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
- V = [Vx;Vy];
16
+ dx = LM(1)-x(1)
117
- // V=xg/100 + w*rand(2,"normal");
118
- endfunction
119
-
120
- function x1 = f(x,v)
17
+ dy = LM(2)-x(2)
121
- global a;
122
- x1 = x + v*a;
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
- Ht = [
19
+ Ht = [
147
-
148
- -(LM(1)-x(1))/(r2)^0.5, -(LM(2)-x(2))/(r2)^0.5;
20
+ -dy/(r2)^0.5, dx/(r2)^0.5;
149
- -(LM(2)-x(2))/(r2), -(LM(1)-x(1))/(r2)];
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
- (3次元の姿勢推定は微小変化でも値がホイホイ飛ぶオイラー角より連続的に変化するクォータニオンの方を採用することが多いわけで。)
26
+ [参考](http://www.namerikawa.sd.keio.ac.jp/publications/domestic_conf/2009/movic2009yama.pdf)
176
-
177
- だから実際にセンサで得られる観測がランドマーク基準の極座標(r, θ)であっても、
178
- 直交座標に変換したものを観測としたほうが計算は楽です。

8

書き忘れ変更点追記

2019/12/05 05:49

投稿

ozwk
ozwk

スコア13553

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

修正

2019/12/05 03:17

投稿

ozwk
ozwk

スコア13553

answer CHANGED
@@ -1,7 +1,7 @@
1
1
  ![イメージ説明](2f063477e615a8dba374c347a970ac03.png)
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

2019/12/05 03:13

投稿

ozwk
ozwk

スコア13553

answer CHANGED
@@ -1,7 +1,7 @@
1
1
  ![イメージ説明](2f063477e615a8dba374c347a970ac03.png)
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

追記

2019/12/05 00:25

投稿

ozwk
ozwk

スコア13553

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

4

修正点

2019/12/05 00:23

投稿

ozwk
ozwk

スコア13553

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

3

暫定

2019/12/05 00:18

投稿

ozwk
ozwk

スコア13553

answer CHANGED
@@ -1,7 +1,5 @@
1
1
  ![イメージ説明](2f063477e615a8dba374c347a970ac03.png)
2
2
 
3
- * Htのミス修正(Ht(2,2)成分の^2の位置がおかしかった)
4
- * Ftを単位行列に変更
5
3
 
6
4
  ```scilab
7
5
  function [] = VectorFieldKalmanFilterLocalization()

2

回答追加

2019/12/05 00:15

投稿

ozwk
ozwk

スコア13553

answer CHANGED
@@ -1,4 +1,170 @@
1
+ ![イメージ説明](2f063477e615a8dba374c347a970ac03.png)
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

2019/12/05 00:13

投稿

ozwk
ozwk

スコア13553

answer CHANGED
@@ -6,5 +6,5 @@
6
6
 
7
7
  (3次元の姿勢推定は微小変化でも値がホイホイ飛ぶオイラー角より連続的に変化するクォータニオンの方を採用することが多いわけで。)
8
8
 
9
- だから実際にセンサで得られる観測がランドマーク基準の極座標(r, θ)だからといっても、
9
+ だから実際にセンサで得られる観測がランドマーク基準の極座標(r, θ)であっても、
10
10
  直交座標に変換したものを観測としたほうが計算は楽です。