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-.

" , #! +

, -$ ,% " # # %- -,- # .-$1

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

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

-.-$ . .-$ % '+567

(

2 2 2

)

2 # * +

* _+' =

2 2 2 2 2

2 * 3 +

* +' = + + 88888888+++3'

$ ! . .$! - % # -/ " .$! - $ 1

!% /-$ $ / !$% , -.-$ . .-$ % 9+

0 - " 2 $ : $ -# # ,-# $! .

0 - " 2 $ : $ -# # ,-# $!

0 - -, %. ;!"!$

( )

+

=

*

(

+

_$

+

₂

)

=

4

₂ # < #+ 88++39 , % $ # =# . " - " %- "" #

.-$ % >5' ?7

(

2 2

)

+

2

+

+

4

=

=

    

  

   

 

+    

 

+

= _{∫ ∫}

+

+ 2

2 2

2

2 2 5 ) 2 5 )

4

' '

3 3 ' 3 3 3 '

3 ξ ξ ξ ξ η η η η

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

- " # %-$ . # 0 " , " %- " 1

/ "# $ , % # ω - " #-, $

! @+ $ .-$ % ' -$, / A ! @

/-, % / !/ -$B - "" # %-, # #

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

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

-$B -.-$ . .-$ % ?+

(

2 2

)

2

*

&

-

=

2 2 2 2

6

*

&

-

=

+

++++++++++++++++++++++++++++++++++++++++++++++++3?

$ ! . .- " # $ % # -/ " .$!

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

( )

&

=

*

( )

&

_$

&

₂

=

7

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

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

, , " /-/ - "" , % %%! $

, !,+

( )

+

_$

&

₂

=

*

# -% #+ +++++++++++++++++++++++3)

, - % !

*

C

₂ . 0 , -$ ,% - #

$ - % ' !%$ %$

*

C

₂ -$ -,- $

.- " # $ -/- $

-

₂ - % .- " # $

− 2

*

6C

-.-$ . .-$ % D+

(

−

)

−

₊

₋

=

_{2 2 2 2}

2

*

-

6

*

*

C

C

C

++++++++++++++++++++++++++++++++3D

(

−

)

−

₊

₊

₋

=

_{2 2 2 2 2 2}

2

*

6

*

&

6

*

-, , $ .- " # $ -/- $

-

₂

- % .- " # $

6C

*

−₂ -/ -/ " %! $)

.- " # $ $ & $ + : # , %! $)

, !, % # , %- 2 ## / A ,

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

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

0 , -$ ,% . , , ₂ - "" - 1

% ' !%$ %$ , !. % , %-%. !%% %

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

, # . %- - # - % " %-%1

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

%- . # #% % % , . / , , ₂

-/ " /-$ # +

(

₋

)

−'

−

₊

=

_{2 2 2 2 2 2}

2

8

6

6

8

6

7

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

,

8

₂ % % , -/ " /-$ #

(

₋

)

−

=

_{2 2 2}

2

6

8

8

++++++++++++++++++++++++++++++++++++++++++++3(

$- # , #!$!, - " $ ! "

,

+

_'≠2 % # , - % '%$ %$ /-$ # .

.-$ % '* " .-$!,- - " %- " , "#

$ !

+

₂ $ .-$ % '

2 2 2 2

2

*

3

*

C

−_+'

=

C

+

++++++++++++++++++++++++++++++++++++++++3'* , & %$ ! $ !%% %

2 2 2 2

2

8

4

8

−_+'

=

+

+++++++++++++++++++++++++++++++++++++++3''

-, $ - !$ " $! 4!.- , /-$ . -" "

" %- # /- $ #-4-. + ;- $

$ #-, $ - !$ . " - " 4 $

%- " -"$ ,# - "" / " $ ,

#-4-. #-, $ .-$, #

. %/ $ '+

'

%/ $ '+ & / " $ ,

#-4-. +

, % # 3 , #-4-.

#-, $ 3θ , + , #-4-.

%., " - " .-$ ! - %., " + & / "

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

., 4- .-$, # . .-$ % '9+

3 3 =

θ

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

( )

=

3

θ

++++++++++++++++++++++++++++++++++++++++++++++++++3'>

-$ % '> / #-%/ , #- # A A #

- "" %- 2 F

( )

) )

=

3

θ

++++++++++++++++++++++++++++++++++++++++++++++++++++++3'?

-$ % '? / #- , % /- # #$

- "" %- 2 F

2 2 2+ −

θ

₌

θ

_'

2 2

2+

−

θ

=

θ

'

2 2 2+

=

θ

+

θ

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

$ !. , #-4-. 3

+

₂ , %

) " B $ /-, % # .-$ % '6 . , # -/ " /-$ #

2 2 2

2+

=

θ

+

+

+

θ

_'

-0 # B-# !$ !@ /-$ .

2 2

*

=

θ

$ .-$ % ' .-$ % ? . , #

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

.- " # $ #-, $ ,

2 2 2

2

*

+

*

_+'

=

+

+

++++++++++++++++++++++++++++++++++++3')

2 2 2

*

&

-

=

+

++++++++++++++++++++++++++++++++++++++++++++++++++3'D

. & %$ ! $ $ !.$! - . " - "

%- "" # .-$ % > - " %-%.-$1

# . %/ $ ' - " !

*

=

θ

*

-

=

+ : #

5

3

, 0 " " %- " / "1

#

*

% # ,

5

3

=

'

(

3

=

'

+

( )

*

'

*

'

4

₂

=

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

(

)













=

_∫

(

+

)

_∫

(

+

)

*

*

* *

'

'

3

ξ

3

ξ

ξ

3

η

3

η

η

(

*

*

)

=

₊

_∫

)

_∫

)

* *

9 '

'

σ

'

+

ξ

+

'

+

η

(

)

9 9

' '

*

*

=

σ

₊

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

0 , -$ ,% " .-$, # . %/ $ 9

/-$ $# .-$ % ' .-$ % ?

.-$ % D .-$ % E .-$ % ( .-$ %

'* .-$ % ''+5> 67

'

−

2

'

−

2 2

6

2

6

2 2

*

'

−

2

* *C2−'

2

*C

−

2

*

C '

−

2

*

2

&

-2

+

'

−

/

'

−

/

%/ $ 9+ & / " $ .$! - .- " # $

;- $# / " $ .$! - .- " # $

0 , -$ ,% . %/ $ 9 % # . /

"$ % , $ .$!"$ % % , -.-$ . %/ $

>+

2 2 2 *C 8

− + −

+' '

C2 82

*

− + −

+' '

C2 82

*

%/ $ > "$ % , $ .$!"$ % % , +

A , .$!"$ % 2 , # -%. . $ %- -$

" $ % ## $ %. , ,

.-$ " A # %., " $ -B $

$ ! .$! - $ -B $ $ !

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

#-4-. / - . ' $ G - #+

. & %$ ! $ !%% % .- " # $ & %$ !

$ $ !.$! - . , # -/ " /-$ #

# < -B ! - - "H9I

=# < -B ! - $! - H9 5 H97I

, A , # - % - % $ #

& %$ ! , -/ " /-$ # + @ < @I

< =#I

, % $ # % $ # ; % $ # & , +

'

=

3

=

6

=

'

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

/-$ # < 5'7I / < 5 7I

< 5'7I

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

! ( $ ,% % $ # & %$ ! ,

-/ " /-$ # +

! - $! - < -B ! - $! - 5 $ 7I

@ < @ J / J ! - $! - I

! - - " < -B ! - - " $ I < @ J ! - - "I

@ < @ J I

< K B3 I

< 1 I

- " , -/ " /-$ #

< 1 @ I

< K J #I

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

@ < @ 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 - ' $ +

+ 3 .- " # $ - % +

/+

4+ %% %.- " # $ - % +

+

%/ $ ? & , % , # $ -B $ !

.$! - '* $ G - $ -B $ !

+ 3 .- " # $ - % +

/+ %% %.- " # $ - % +

%/ $ 6 & , % , # $ -B $ !

.$! - '* $ G - $ -B $ !

.- " # $ ' $ +

/-$ , $ -B $ !.$! - '*

$ G - , $ -B $ !.- " # $

'* $ 3 %/ $ ? % # -$, / A ,

!%% % .- " # $ % # % , %. 9E $

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

$ -B $ ! .$! - " % - "#

, $ ! .- " # $ ' $ 3 %/ $ 6 % #

!%% % .- " # $ % # % , > 9 $

!%% %.-$# $ % # % , 9 > $ + & ,

. # # / A 2 # -% # #-4 , $ !

.- " # $ % # !%% % .- " # $ !%% %

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

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

- % %- -# - " , -/- $ + - "

# , . # # / A 2 # -%

" / # # %-%. $ ! .- " # $ "

#-4 , # $- , .- " # $ # %- -#

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

# # / A , - % - " %- "" #

0 , -$ ,% # ,-/ / # 2 # $ %-

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

+ 3 .- " # $ - % +

/+ %% %.- " # $ - % +

%/ $ ) & , % , # $ -B $ !

.$! - ' $ G - $ -B $ !

.- " # $ '* $ +

%/ $ ) .-$, # , .- " 2 - "

, $ -B $ ! .$! - , $

-B $ !.- " # $ % "1% " ' $ G -

'* $ + %/ $ D .-$, # ,

.- " 2 - " , $ -B $ ! .$!

-, $ -B $ ! .- " # $ % "1

% " ' $ G - ' $ + /-$

, $ -B $ ! .$! - ' $ G -

, $ -B $ ! .- " # $ '* $

3 %/ $ ) % # -$, / A , !%% %

.- " # $ % # % , %. 9> $ !%% %

- % % # % , %. 9 $ + $

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

$ -B $ ! .- " # $ ' $ 3 %/ $

D % # !%% %.- " # $ % # % , > $

!%% %.-$# $ % # % , * E $ + & ,

. # # / A 2 # -% # #-4 , $ !

.- " # $ % # !%% % .- " # $ !%% %

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

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

- % %- -# - " , -/- $ + - "

# , . # # / A $ %-

" / # # %-%. $ !.- " # $ "

#-4 , # $- , .- " # $ # %- -#

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

# # / A , - % - " %- "" #

0 , -$ ,% # ,-/ / # 2 # $ %-

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

/+ %% %.- " # $ - % +

%/ $ D & , % , # $ -B $ !

.$! - ' $ G - $ -B $ !

.- " # $ ' $ +

$ , % , - " /-$/ " .-$ /

$ -B $ !.- " # $ $ -B

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

'+ - "" 0 , -$ ,% . % , % 1

# " $! 4!.- . %-$- # $ ! "

. , % # $ ! " .

.- " # $ - "" , - % %- -1

# - " , + %% % . - %

" #-4 ,+

9+ , $ -B $ $ ! .$! - #

/ # /-$.- " $ -$ . , .- " # $

, - % - "# , $

-B $ $ ! .- " # $ " /-$.- " 1

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

>+ & , % , - " -B $ $ $ !

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

# $ , - % %- 2 $ ,

2 # -B $ $ $ !.- " 1

# $ #-4 , % # , .- " # $ ,

- % # %- -# - " , -/-1

$ +

?+ -% " / # # %-%. $ !

.- " # $ " #-4 , # $- , .- " # $

# %- -# - " , -/- $

# %-%/-$ # , - % " ,-/ / #

2 # %- "" # 0 , -$ ,% +

'+ ;$!A !/-$ $!B-$ '((D L % ) $

7 ) " $( ) '' $!) "

#$ !%$ (M :! ,,- N ! $

- ! +

9+ $- ! -! - 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+

6+ %! 9**' L " 9$ !%$ (M 4 1

-% 4+4 ! !+- + -,4 $-" $

; !. 9**) L % ) $ !

" 9$ !%M AAA+4 + 4+- GOA-,4

-. $ -%- !0 !%. -$ 4 -