Simulasi Filter Kalman Untuk Estimasi Su

!

! "#! $ %! ! &

!
" #$ !% $
%! % $&!
$
! '% ! $ ! % #$ !%$ ( '% !"
%!" &! ! $ !
#% " $(
) !
! $ # %" $
!#
! '% !
!
" #$ !% "
! ''% *$" !)
+ !, $
# $ ! % - !"
! !, $

)
' !, $
. "'
$
#
" #$ !% $
"$ $"$/!
! "!
# ! , %! !%% %
$ ' '!% !*' %! !
$
!'
# !
" #$ !%
( %$ " ) $"
!$
#$ !% )
# (-%
'! (! %
$

! "! %!"!
$ ! & %$ !
)! !%"$ !
+"
$ # %" $ #% " ! "' ! $
!) # "! %!"!
$ ! & %$ ! $ $(
+
! "! %!"! $ 0 &!%% !
! '% !
$!
& %$ !
%$ !
! &!%
!% $ - # ! ! $" !
$ $ ))!)
! !%% % & %$ ! " %$* $ !
$"! !' # ! !%% %
& %$ ! " %$* $ "
!

" #$ !% $ %' % !
!
# ! "! %!"! $
! $" ! #
%
$
!- + %) 1
" #$ !% $ ! % - !" (-%
'!

'()*
+ +
,%
%-%. /, # #
% # ,
" %-$ . ,
!,
%! % $&! "
% , 0 , -$ .
.$! " , -$ - "" 0 , -$

-$ -/
#- , - "
% 0 , -$
,% + , -$
,%
. %- " 1, "#
$! $
,
" %- "
" 0!$%
%- " %/ ,
1
0!$%
-$1 -/
# .$! - ,-/ , 2 +
.$! " %- "" # 0 , -$ ,%
# %-%1
0 , -$
$ !
$

.
2#
, %
.-$1
%
.-$ %
!
.-$ %
#-, 1
$ +
"1%
" .-$ %
%-%.
$!
, " /-/
- ""
#
#!$!,
, "
$ #$ !+

& , .-$
"
!. % ,
" " #
#
%- . #
, - %
.
,%
, # #
- "
%-% % ,#
$-$
# $ !%% %+
% ,
.-$,
# $!
3
.
' -$ 2

#-/-/
" , # #
- " %-%/-$ #
,
& %$! ! $
$ ! .- " # $
& %$ !
$
$ ! .$! " /-$/- 1/# %-,
.- " $
$ %
"1%
"
$ !
,
% #
" /-$ #
/-$ . #-4-.
" ,
#!

+

%

, -$
,

,%
- %

#
! .
, % .-$ %
.-$ %
'+567

#
-.-$ .

* 2 +' = # ( * 2

* 2 +' =
$! .
!% /-$

2

"

2

# %- -,.$! -0-$-

#

.-$1
.
, , - $
"

* 2 + 32

2

+ + 2 88888888+++3'

.$! % #
-/ " .$! - $ 1
$/
!$% , -.-$ .
.-$ %
9+
0 - " 2 $: $
0 - " 2 $: $
0 - -,

-# # ,-# $!
-# # ,-# $!
%. ;!"!$

.

$

42 =

+

2

(+ + )

=

2

  2 + '
  ∫ 3 2 + ' ξ 5 3ξ
  2

42 =

# < #+ 88++39
" - " %- "" #

2

.

, % $ # =#
.-$ %
> 5' ?7

2

  2 +'
3ξ ) ξ   ∫ 3 2 + ' η 5 3η
 2


3η ) η 


- "
0 "
,
# %-$ . #
"#
$
, % # ω
! @+
$ .-$ %
' -$,
/-, % /
!/ -$B
- ""
!/ -$B
.-$, #
%! -, .%-%- #
! #- #-, $
-$B
-.-$ .
.-$ %
?+

/





++++++++++++3>

" %- " 1
- " #-, $
/ A
! @
# %-, # #
" # $
"
"
.
!/1

- 2 = (*2 &2 )
- 2 = 6 2 * 2 + & 2 ++++++++++++++++++++++++++++++++++++++++++++++++3?

$ ! .
.- " # $
% #
-/ " .$! % ) " /-$ $ /
!$% , -.-$ .
.-$ %
6+

(& & ) = 7

(& ) = *
,
,

$ 2

$ 2

# -%

, - %

! *C 2 .

- %

.- " # $

6C*

−
2

# < #+ ++++++++++++36

2

% # . , / A .$! - % ) " A
, " /-/
- ""
,
%
%%!
!,+

(+ & ) = *
$

+2 )

(+ + ) = 4

(+ ) = *

0 , -$

-2

-/- $

-.-$ .

#+ +++++++++++++++++++++++3)

!%$ %$ *C2

'

2

*C 2 = *C 2− +

2

,%

-$

-

#

-,-

- %

.-$ %

*C 2 = *C 2− +

B
$

D+

$

.- " # $

(- − 6*C ) ++++++++++++++++++++++++++++++++3D
(6 * + & − 6*C )
−
2

2

2

2

2

−
2

-,

,

$ .- " # $
−

-/
-/
.- " # $
6C* 2
.- " # $
$
& $ + :#
,
!, % #
,
%2 ##
- %
% - "
, .- " # $
, 0 # !$ ( $ .
0 , -$ ,% +

- %

)θ 3
=
)

-2

-/- $

" %! $)
,
%! $)
/ A
+ ,

,

2

(

= 82− 6 2 6 2 82− 6 2 + 72

,

82 =

82 % %

(

−

2

$-

#

,

8

2

2

-/ " /-$ #

,

#!$!,

,

- %
.-$!,-

"

- "

$ !

"

'%$ %$ /-$ # .
- " %- " , "#

'

*C 2 + 32

& %$

=

++++++++++++++++++++++++3E

−
2

$ .-$ %

*C 2−+' =
−
2 +'

)

−'

6 2 8 ++++++++++++++++++++++++++++++++++++++++++++3(

,
+'≠2 % #
.-$ %
'*
$ ! +2

,

)

!

2 ++++++++++++++++++++++++++++++++++++++++3'*
$ !%% %

2

+ 42 +++++++++++++++++++++++++++++++++++++++3''

82

-$ %
'?
/
- "" %- 2 F

θ2+' −θ2

2

0 , -$
,%
.,
,
- "" - 1
2
% ' !%$ %$
, !. % ,
%-%.
!%% %
% % .-$!,- 2 #
" % % %+
,
# %
,
. %- - #
- %
" %-%1
#
.
& %$ ! % % %+ - -,#
%- . #
% %
,
. /,
,
# %
2
-/ " /-$ # +

-, $
- !$ " $! 4!., /-$ . -" "
" %#
/- $
#-4-.
+ ;- $
$ #-, $
- !$ .
" - " 4 $
%- " -"$ ,#
- ""
/ "
$
,
#-4-.
#-, $
.-$,
#
.
%/ $ '+

( ) ++++++++++++++++++++++++++++++++++++++++++++++++++++++3'?

=

#-

, % /-

#$

2

θ 2 +' − θ 2 =
θ2+' =θ2 +

2

+++++++++++++++++++++++++++++++++++++++++++++++++3'6

2

3 +2
'6 .

$ !.
, #-4-.
) " B $ /-, % # .-$ %
-/ " /-$ #

θ2+' =θ2 +
-0

#

%
, #

+ +2

2

#

,

B-# !$

! @ /-$ .

*2 = θ 2
$ .-$ %
'
.-$ %
? .
, #
.-$ %
.$! - , -$
#
!
.- " # $ #-, $
,
*2 +' = *2 + 2 + +2 ++++++++++++++++++++++++++++++++++++3')

-2 = *2 + &2

++++++++++++++++++++++++++++++++++++++++++++++++++3'D
. & %$ ! $
$ ! .$! .
" - "
%- "" #
.-$ %
>
- "
%-%.-$1
# .
%/ $ ' - "
! * =θ
- = * + : # 53
, 0 "
" %- " / "1

* %#

#

42 =

(*'*' )

(*'*' ) =

53

,

'

=

(3 = ' +

+++++++++++++++++++++++++++++++++++++++++++++++++3'E



 ∫ ( 3ξ +3ξ )ξ ∫ ( 3η +3η )η 
*
*


(*' *' ) = σ +9 ∫'+)ξ ∫ +'+)η
(*' *' ) = σ

*
9
+

*
9

'

%/ $ '+ & /
#-4-.

"

$

,
+

, % #
3
,
#-4-.
#-, $
3θ
,
+
, #-4-.
%., " - " .-$ ! - %., " + & / "
$ #-4-.
, % $ 0!$%
., 4- .-$,
# .
.-$ %
'9+

θ3 =

θ3

&2
*2 −'

*2

++++++++++++++++++++++++++++++++++++++++++++++++++++++3'9

()

-$ %
'>
- "" %- 2

++++++++++++++++++++++++++++++++++++++++++++++++++3'>
/
#-%/ , #- # A
A #
F

2

/ −'

62

*2 −'

%/ $ 9+ & /

*C2

-

62

+2
2 −'

3

=

"$ % /,!# / "
$ .$! - .- " # $
0 , -$ ,%
" .-$,
# .
%/ $ 9
/-$
$#
.-$ %
' .-$ %
?
.-$ %
D .-$ %
E .-$ %
( .-$ %
'*
.-$ %
''+5> 67

"
0 , -$

*C2−

/ −'

2 −'

*C 2 −'

$ .$! - .- " # $
,% +

;$#
/ "
$ .$! - .- " # $
0 , -$
,%
.
%/ $ 9 % #
.
/
"$ % , $ .$!"$ % % ,
-.-$ .
%/ $
>+

-$
@

"
@

<
<

'%$ %$
J/
I
K J =#I

,

-/ " /-$ # +

&

, .- " 2
-$
. .$!"$ %
" -,
/
.-$,
# .
%/ $ ? %. - "
%/ $
D+ & , .- " 2
, # #
- "
$ 9* - #
A #
%., " * '
#
,
, # #
" $
" %/ $
, .- " 2
,-/ %
% +
-% .- " 2
, # #
- "
, % #
" %
' $
G -+ - " 2
, # #
# %-,
.- " $
$ ! .$! $ !
.- " # $
-$
.
, .- " # $
,
- %
- ""
# %-,
, -$ -/
, # #
- " %-%/-$ #
,
$ -B
$
$ ! .$! ,
$ -B
$ !
.- " # $
" /-$/- 1/- +
%/ $ ?
.-$,
#
, .- " 2
- "
,
$
-B
$ ! .$! ,
$ -B
$!
.- " # $
%
"1%
" '* $
G '*
$
+
%/ $ 6 .-$,
#
, .- " 2
- "
,
$ -B
$ ! .$! ,
$ -B
$ ! .- " # $ %
"1%
" '*
$
G '$
+

C 2−+' 82−+'
*

2

C2
*

82

C 2−+' 82−+'
*

%/ $ >

"$ % , $ .$!"$ % % ,

+

A , .$!"$ % 2 , #
-%. . $ %- -$
$
% ##
$
%. ,
,
.-$
"
A #
%., "
$ -B
$
$ ! .$! $ -B
$
$ !
.- " # $ +
,
! A , /-$
,
!,
#-4-.
/
- .
'$
G - #+
. & %$ ! $ !%% % .- " # $
& %$ !
$
$ ! .$! .
, #
-/ " /-$ #
# < -B ! - - "H9I
=# < -B ! - $! - H9 5 H97I
,
A ,
#- %
% $#
& %$ !
,
-/ " /-$ # +
@ < @I
< =#I
, % $# % $#;
% $#&
, +
"

=' 3 =

+

3

.- " # $
/+

- %

+

6 ='

- " % $ # -$ -/
.
, #
-/ "
/-$ #
< 5'7I
/ < 5 7I
< 5'7I
-$
"
,
! #-, $
.$! - - %
! ( $
,%
% $ # & %$ !
,
-/ " /-$ # +
! - $! - < -B ! - $! - 5 $
7I
@<
@J/
J ! - $! - I
! - - " < -B ! - - " $
I
<
@ J ! - - "I
@ +
/+ %% % .- " # $
%/ $ D & , % ,
.$! - ' $
G .- " # $

- %
#
'$

+

$ -B
$!
$ -B
$!
+

?+

6+
$
$
'+

9+

>+

?+

, % ,
- "
/-$/ "
.-$ /
$ -B
$ ! .- " # $
$ -B
! .$! .
%. ,#
-/ " /-$ # +
- ""
0 , -$
,% .
% ,
% 1
# " $! 4!.- . %-$- #
$!
"
.
, % #
$!
"
.
.- " # $
- ""
, - %
%- -1
#
- "
,
+ %% % .
- %
" #-4 ,+
,
$ -B
$
$ ! .$! #
/
# /-$.- " $
-$
.
, .- " # $
,- %
- "#
,
$
-B
$
$ ! .- " # $
" /-$.- " 1
$
-$
.
, .- " # $
,- % +
& , % ,
- "
-B
$ $
$!
.- " # $ /- $ %-%.-$,
#
, .- " 1
# $
, - %
%- 2
$
,
2 # -B
$ $
$ ! .- " 1
# $
#-4 , % #
, .- " # $
,
- %
#
%- -#
- "
,
-/-1
$ +
-%
" / # # %-%.
$ !
.- " # $
" #-4 , # $, .- " # $
#
%- -#
- "
,
-/- $
# %-%/-$ #
,- %
" ,-/ / #
2 # %- "" # 0 , -$ ,% +

;$!A

!/-$ $!B-$ '((D L % ) $
7 ) " $(
) '' $!)
"
#$ !%$ (M :!
,,- N !
$
- !
+
$- !
-! - 9**9 L '' $ $
#
"
9$ !%
( %$ "
7
:
$ $
) ; % ) < )! $ (M ,-4 $ 4 ,
"1
--$ "
, - $ $!2-4
B-$
!0
-A4 ,$ , +
$-A ,
!
-$
"
+
$-A
9**' L " 9$ !%$ (M :!
,,- N
!
$ - !
+
,%
'()* L
!+ '%
$ ! %
#$ !%$ (
) '%!)$ $
'% !"M $ 1
4 ! !0 1:! $ , !0 / 4
- " --$ " -$ E9 >?1?6+
%!
9**' L "
9$ !%$ (M 4 1
-% 4+4 ! !+- + -,4
$-"
$
; !. 9**) L
% ) $
!
" 9$ !%M AAA+4 + 4+- GOA-,4
-. $ -%!0
!%. -$
4 - 4B-$
!0 !$
$!,
.-,& ,