2018年度センシング工学１４回目のカルマンフィルタ講義資料。 最小二乗法の加重平均から紐解く試み 有本卓「カルマン・フィルタ」（産業図書）の導出過程を参考に。

1. 2018
14

2. (3)

3. n
x0
1 1
x0
(t,x) = (1, -1), (2, 1), (4, 4
x =
t2
4
+ x0

4. n t=1,t=3,t=4
t=2
¨t=1 t=3 t=4
n
¨ t=1 t=4
t=3
¨

5. 2
1
1 1
,
i
iN
i i
N
i ii
wav w
w
xw
x
s
==
å
å
=
=
Nixx ii ,,1 !=±= s

6. x =
t2
4
+ x0
x0 = x −
t2
4σx
2
=1, σt
2
=1
∂x0
∂x
=1,
∂x0
∂t
= −t / 2
∴σx0
2
=
∂x0
∂x
$
%
&
'
(
)
2
σx
2
+
∂x0
∂t
$
%
&
'
(
)
2
σt
2
=1+t2
/ 4
x0 t
→

7. (t,x) = (1, -1), (2, 1), (4, 4)
ˆx0
(1)
= −1−12
/ 4 = −5 / 4, σx0
2(1)
= 5 / 4
ˆx0
(2)
=1− 22
/ 4 = 0, σx0
2(2)
= 2
ˆx0
(1:2)
= ((−5 / 4) / (5 / 4)+ 0 / 2) / (4 / 5+1/ 2) = −10 /13 σx0
2(1:2)
=10 /13
ˆx0
(4)
= 4− 42
/ 4 = 0,σx0
2(4)
= 5
ˆx0
(1,2,4)
= (−10 /13/ (10 /13)+ 0 / 5) / (13/10 +1/ 5) = −2 / 3= −0.67(= −0.7)
σx0
2(1,2,4)
=10 /15 = 0.67(= 0.7)
t=1 x=-1
t=2 x=1
t=4 x=4
σx0
2
=1+t2
/ 4
x0
(1,2,4) = -0.7 0.8

8. n t=3
¨ t=1,2,4
¨ t=2,4 t=1
→
n t=3 x3
¨
x3 =
32
4
+ ˆx0
(1,2,4)
∂x3
∂ˆx(1,2,4)
0
=1
x =
t2
4
+ x0
x3 =
9
4
+ x0 ∴x0 = x3 −
9
4
∴x3 = x −
t2
4
+
9
4
2

9. n
¨
!"($)
&'(())
*
¨
!"(+:$-+)
&'((.:)/.)
*
n
¨

10. n
¨
¨
n
¨
n

11. n x,y
σx
2
σxy
σxy σy
2
!
"
#
#
$
%
&
&
(x,y)
(determinant)
σx
2
σxy
σxy σy
2
=σx
2
σy
2
−σxy
2
≥ 0
Schwarz
(

12. !"
#
"$
%$ & = 0:
)
*
"+,- = ."+ + 0+
1
!"
%
& &
&

13. x2
x1

14. n
!"#$ = &"!" + ("
(" 0, *+,
-
&" *+,
-
k
E[!0] = !0
V !0 = *34
-
k k+1

15. n
k-1 !"#$% &'()*
+
k ,"# -'(
+
,"# = /#$% !"#$%
-'(
+
= /#$%
+
&'()*
+
+ &1()*
+
"# 2# ,"# 2#

16. n
!" = $"%" + '"
'" 0, )*+
,
$" )*+
,
k
-%" = ."!" + /"
V -%" =E -%" − %"
,
= )3+
,
." /"

17. n
!"# = %#&# + (#
V !"# =E !"# − "#
,
= -./
,
E[1#] = 0 →
V[1#]→ min →
1# = !"# − "#

18. !"# = %#&# + (#
V !"# =E !"# − "#
,
= -./
,
E[1#] = 3[!"# − "#] = 0
3[!"# − "#] = 3[%#&# + (# − "#]
= 3 %#5#"# + %#6# + (# − "#
= %#5# − 1 3 "# + 0 + (#
= %#5# − 1 8"# + (# = 0
(# = 1 − %#5# 8"#
&# = 5#"# + 6#

19. !"# = %#&# + (#
V !"# =E !"# − "#
,
= -./
,
V[1#]→ min
V 1# = 3[(1# − 3 1# ),
]
= 3 ( %#6# − 1 "# + %#8# + (# − %#6# − 1 9"# − (#),
= 3 ( %#6# − 1 ("#−9"#) + %#8#),
= %#6# − 1 ,
: "# + %#
,
-;/
,
= %#6# − 1 ,
<./
,
+ %#
,
-;/
,
2 "# 8# 1# "#

20. !"# = %#&# + (#
V !"# =E !"# − "#
,
= -./
,
V[1#]→ min
V 1# = %#3# − 1 ,
5./
,
+ %#
,
-6/
,
7V 1#
7%#
= 23# %#3# − 1 5./
,
+ 2%#-6/
,
= 0
%# =
3#5./
,
3#
,
5./
,
+ -6/
,
, (# = 1 −
3#
,
5./
,
3#
,
5./
,
+ -6/
,
;"#

21. !"# = %#&# + (#
V !"# =E !"# − "#
,
= -./
,
%# =
0#1./
,
0#
,
1./
,
+ -2/
,
, (# = 1 −
0#
,
1./
,
0#
,
1./
,
+ -2/
,
5"#
!"# =
0#1./
,
&# + -2/
,
5"#
0#
,
1./
,
+ -2/
,
=
0#
,
-2/
,
&#
0#
+
1
1./
, 5"#
0#
,
-2/
, +
1
1./
,
6/
7/
5"#
&# = 0#"# + 8#
9["#] = &#/0#
= "# =
= &#
0#
, =
-2/
,
0#
,

22. !"# = %#&# + (#
V !"# =E !"# − "#
,
= -./
,
!"# =
0#1./
,
&# + -2/
,
3"#
0#
,
1./
,
+ -2/
,
=
0#
,
-2/
,
&#
0#
+
1
1./
, 3"#
0#
,
-2/
, +
1
1./
,
5 !"# = -!./
,
=
6 !"#
6&#
,
-2/
,
+
6 !"#
6 3"#
,
1./
,
= 1/
0#
,
-2/
,
+
1
1./
,

23. :
n : "# , $%&
'
n (") = +),- ."),-, 0%1
'
= +),-
'
$%123
'
+ $5123
'
n
¨ .") =
61781
9 :1;<=1
9 (%1
61
9
781
9 ;<=1
9 =
>1
9
?=1
9
@1
>1
;
3
A81
9 (%1
>1
9
?=1
9 ;
3
A81
9
¨$%1
'
=
B .%1
B:1
'
$C1
'
+
B .%1
B (%1
'
0%1
'
= 1/
61
9
<=1
9 +
-
781
9

24. 2
n
n !=($%, $')
)[!] 2 2 +
n → +,%

25. 2
!"#$ = &"!" + ("
(" ), *"
E[!-] = !-
V !- = 0-
!-2
1
0-2
2
!"#$2
1
&"2
2
*"2
2
!"2
1
= + ("2
1

26. 2
!"#$ = &"!" + ("
(" ), *"
+!" = &",$-!",$
." = &",$/",$&",$
0
+ *",$
&",$2
2
*",$2
2
= +."2
2
&",$
0
2
2
/",$2
2

27. !" = $"%" + '"
'" 0, )*+
,
-%" = ."!" + /"
V -%" =E -%" − %"
,
= 3"
2
4"1
1
$"1
2
%"
2
1
= + '"1
1
4"1
1
."
1
2
= +
-%"2
1
/"
1
2

28. 2
E[#$] = '[()$ − )$] = 0
'[()$ − )$] = '[,$-$ + /$ − )$]
= ' ,$0$)$ + ,$1$ + /$ − )$
= ,$0$ − 1 3)$ + /$ = 0
/$ = 4 − ,$0$ 3)$
()$ = ,$-$ + /$
V ()$ =E ()$ − )$
6
= 7$
0$1
2
,$
1
2/$
1
2 3)$2
1
4
2
2=
-
-[ ]

29. V[#$]
V #$ = '[(#$ − ' #$ )(#$ − ' #$ )+
]
= (,$-$ − 1)/$ ,$-$ − 1 +
+ ,$123
4
,$
+
2
56$ = ,$7$ + 8$
V 56$ =E 56$ − 6$
4
= :$
-$
,$ ;[ ] /$ -$
<
,$
T
;[ ]
+ ,$
,$
T123
4
V #$2
2
=

30. V[#$]
V #$ = '[(#$ − ' #$ )(#$ − ' #$ )+
]
= ,$-$ − 1 /$ ,$-$ − 1 +
+ ,$123
4
,$
+
= ,$ − 5$ -$/$-$
+
+ 123
4
,$ − 5$
+
+ 6$
5$= /$-$
+
-$/$-$
+
+ 123
4 78
6$ = (9 − 5$-$)/$
,$ = 5$ ( ) 6$
2
:;$ = ,$<$ + =$
V :;$ =E :;$ − ;$
4
= ?$

31. !"= $"%"
&
%"$"%"
&
+ ()*
+ ,-
." = (0 − !"%")$"
= $" − $"%"
&
%"$"%"
&
+ ()*
+ ,-
%"$"
." = ($"
,-
+ %"
&
()*
,+
%"),-
(= 3")
2
45" = !"6" + 7"
V 45" =E 45" − 5"
+
= 3"

32. !"# = %#
&'
+ )#
*
+,-
&.
)#
&'
%#
&'
/"# + )#
*
+,-
&.
0#
1# = (%#
&'
+ )#
*
+,-
&.
)#)&'
0# = )#"#(+4#: ) 0# )#
*
+,-
&.
)#
2
!"# = 6#0# + 7#
V !"# =E !"# − "#
.
= 1#
!"# %#
&'
)#
)#
;
+,-
.
/"# )#
;
+,-
&.
<#
)#
)#
;
+,-
&.
1#
%#
&'
%#
&'

33. 1-40
! =
cos 20° −sin 20°
sin 20° cos 20°
, = 1 0

34. 1-10
! =
cos 20° − sin 20°
sin 20° cos 20°
, = 1 0
x y

35. 10-20
! =
cos 20° − sin 20°
sin 20° cos 20°
, = 1 0

36. 20-30
! =
cos 20° −sin 20°
sin 20° cos 20°
, = 1 0

37. 30-40
! =
cos 20° − sin 20°
sin 20° cos 20°
, = 1 0

38. y
y

39. ! =
cos 20° − sin 20°
sin 20° cos 20°
, = 1 1
. = /0 + /2