Regelung Mechatronischer Systeme, Regelungs- und Systemtechnik 3

Regelung Mechatronischer Systeme, Regelungs- und Systemtechnik 3

Kapitel 3: Kalman-Filter

Prof. Dr.-Ing. habil. Pu Li

Fachgebiet Prozessoptimierung

Diskrete dynamische Systeme

) 0

0 ( )),

1 (

), ( ( )

1

(k f x k u k x x

x + = + =

Die Modellgleichung (nichtlinear):

Die Modellgleichung (linear)

) (k Zustandsvariable: x

Sie ist kontinuierlich aber nicht differenzierbar.

) (k Steuervariable: u

Sie ist stückweise konstant.

) 0

0 ( )),

1 (

) ( )

1

(k ax k bu k x x

x + = + + =

Zustandsraumdarstellung diskreter Systeme

k k

k

k

Ax Bu w

x =

₋₁

+ +

k k

k

Cx v

y = +

w

k

v

k

Strukturbild:

Unsichere (stochastische) Störungen:

Prozess : Ungenauigkeiten des Modells

: Ungenauigkeiten der Messung

) ,

(

~ 0 Q

w

_k

N

) ,

(

~ 0 R

v

_k

N

Wie beschreibt man die Störgrößen?

Eine Zufallsvariable:

• Sie hat einen Bereich, in denen sie ihren Wert nimmt.

• Ihren Wert kann man nicht vorhersagen.

• Durch Beobachtung erhält man die Wahrscheinlichkeitsverteilung.

Auslegung eines Beobachters unter den Störungen

• werden gemessen.

• Störgrößen sind unbekannt.

• Zustandsgrößen sind abzuleiten.

x

_k

y

k

k k

v

w ,

Messwerte zweier Zufallsvariablen:

i ξ_1,i

n n_1,i

∑= i

j i

n n

1 , 1

i ,

ξ2

n n_2,i

∑= i

j i

n n

1 , 2

1 [0 250) 0,01 0,01 [0 350) 0,08 0,08

2 [250 260) 0,05 0,06 [350 360) 0,10 0,18

3 [260 270) 0,09 0,15 [360 370) 0,12 0,30

4 [270 280) 0,12 0,27 [370 380) 0,14 0,44

5 [280 290) 0,16 0,43 [380 390) 0,15 0,59

6 [290 300) 0,17 0,60 [390 400) 0,13 0,72

7 [300 310) 0,14 0,74 [400 410) 0,10 0,82

8 [310 320) 0,11 0,85 [410 420) 0,07 0,89

9 [320 330) 0,07 0,92 [420 430) 0,05 0,94

10 [330 340) 0,05 0,97 [430 440) 0,03 0,97

11 [340 350) 0,02 0,99 [440 450) 0,02 0,99

12 [350 500) 0,01 1,00 [450 600) 0,01 1,00

Wahrscheinlichkeitsdichte:

Wie häufig (wie dicht) in einem Intervall?

Die Dichtefunktion:

i i

n

n n

i

ξ

ξ ρ

ξ

∆

 

 





=

→

∆→∞ 0

lim )

(

0 )

(ξ >

ρ

Charakter der Dichtefunktion:

1 )

( =

∫

∞

∞

−

ξ ξ

ρ d

) (ξ ρ

ξ

Wahrscheinlichkeitsverteilung:

{ ^{≤ <} } ⁼ ∑ ⁼ ∑ ^{∆ =} ∫

→

∆→∞

→

∆→∞

i

i i i

b

a j

j n j

j

j i n

i

d

n b n

a

P ξ ρ ξ ξ ρ ξ ξ

ξ ξ

) ( )

( lim

lim

0 0

Unendliche Beobachtungen:

Relative Häufigkeit = Wahrscheinlichkeit d.h.

{ }

¹

0 ≤ P a_i ≤ξ < b_i ≤ Charakter der Wahrscheinlichkeit:

{

^a_i ^{≤ < b}_i+1

} {

^{= P a}_i ^{≤ < b}_i

} {

^{+ P a}_i+1 ^{≤ < b}_i+1

}

P ξ ξ ξ

{

^{− ∞ <}

^ξ

^{< ∞}

}

⁼¹

P

Verteilungsfunktion:

Wie groß ist die Wahrscheinlichkeit in einem Intervall?

{ ξ

z

}

F z

ρ ξ

d

ξ

P

z

) ( )

(

∫

∞

−

=

=

<

{ ^a

_i

^{≤ < b}

_i+1

} { ^{= P a}

_i

^{≤ < b}

_i

} { ^{+ P a}

_i+1

^{≤ < b}

_i+1

}

P ξ ξ ξ

Weil

{ } { }

i i

i i

i n

i

i i

n n

b a

P b

P

i

ξ ξ

ρ

ξ ξ

ξ

∆

=

=

<

≤

=

<

∑

∑

∑

=

→ =

∆→∞

=

) ( lim

10

1 10

0 1

10

1 10

Die kontinuierliche Funktion:

ξ

i

i ξ

ξ

ρ ∆

∑

^{( )}

Normalverteilung:

∞

<

<

∞

 −



 



− −

= ξ

σ µ ξ σ

ξ π

ρ ₂ ²

2 ) exp (

2 ) 1

(

∫

∞

− 



 



− −

=

<

= P z ^z d

z

F ξ

σ µ ξ σ

ξ π ₂ ²

2 ) exp (

2 } 1

{ )

(

) ,

(

~ µ σ

²

ξ N

{ }

{ }

{

³²

}

^00,,99739545

6827 ,

0

≈

<

−

≈

<

−

≈

<

−

σ µ

ξ

σ µ

ξ

σ µ

ξ P P P

Eine Zufallsvariable ist normalverteilt, wenn sie eine Summe von

mehreren kleinen Zufallsvariablen verursacht ist (nach dem Zentral-Limit- Theorem), nämlich

ξ

N

ξ ξ

ξ =

1

+

2

+  +

ξ

Standardisierung einer Normalverteilung:

) 1 ,

0 (

~ )

, (

~ N µ σ

²

ξ

_S

N

ξ ⇒

∫

∫

∫



 



 −

∞

−

∞

−

∞

−



−

 =



 



 −











 



 



−  −

=



 



 −

−

=

=

<

σ µ

ξ π ξ

σ µ ξ

σ µ ξ

π

σ ξ µ ξ

σ ξ π

z

S S

z

z

d d

d z

F z

P

2 2

2 2

2 exp 1

2 1 2

exp 1 2

1

2

) exp (

2 ) 1

( }

{



 

 Φ −

=

< σ ξ z ^z µ P{ }

Berechnung der Wahrscheinlichkeit mit vorhandener Software:

Weil

Gleichverteilung:









>

<

≤

− ≤

=

b ,

wenn 0

1 wenn )

(

ξ ξ

ξ ξ

ρ

a b a a

b







>

≤

− <

− ≤

=

b wenn

1

a wenn

wenn 0

) (

z

b a z

b a z

a z

z F

Erwartungswert:

j N

j

j j

N

j

j N j

j

j j n

i

i n

n n n

n ξ ξ ξ ρ ξ ξ ξ

ξ

∑ ∑ ∑ ∑

=

=

=

=

∆

=

=

=

=

1 1

1 1

) 1 (

1

Wo liegt das Zentrum der beobachteten Daten?

Der Mittlerwert:

∞

∫

∞

−

= Ε

=

→

∆

∞

→ ξ ξ ξ ξ ρ ξ dξ

n , 0: ( ) ( )

Varianz:

Wie groß ist die Streuung vom Erwartungswert?

[ ] _∫

^∞

∞

−

−

=

− Ε

=

ξ ξ ξ ξ ρ ξ ξ

ξ

d

D( ) ( )² ( )² ( )

∑

∑

∑

= = =

−

=

−

=

−

= ^N

j

j j N

j

j j n

i

i n

n n n

D n

1

2 1

2 1

2 1 ( ) ( )

) 1 (

)

(ξ ξ ξ ξ ξ ξ ξ

Durch Meßdaten:

Durch Integration:

Erwatungswert und Varianz:

Normalverteilung:

ξ

=

µ

, D(

ξ

) =

σ

²

Gleichverteilung:

ξ

= (b + a) / 2, D(

ξ

) = (b − a)² /12 Standardisierung einer Zufallsvariable:

) (

) ( ξ

ξ ξ ξ

S

D

Ε

= −

Damit

Ε ( ξ

_S

) = 0 1 )

(

_S

=

D ξ

Multivariate Stochastik:

T m

] [ ξ

₁

_ ξ ξ =

Es gibt mehrere unsichere Variablen:

Wahrscheinlichkeitsverteilung:

? }

, ,

{ )

, ,

( z

₁

z

_m

= P

₁

< z

_{1 m}

< z

_m

=

F _ ξ _ ξ

Man definiert eine gemeinsame Dichtefunktion: ρ⁽ξ₁^,_^,ξ_m^{) ≥ 0}

∫ ∫

∞

∞

−

∞

∞

−

=1 )

, ,

(

ξ

₁

ξ

_m d

ξ

₁ d

ξ

_m

ρ

_{ }



m m

z z

m

d d

z z

F

m

ρ ξ _ ξ ξ _ ξ





1 1

1

, , ) ( , , )

(

∫ ∫

1

∞

− −∞

=

damit

Die Wahrscheinlichkeitsfunktion:

Multivariate Stochastik:

T m

] [ ξ

₁

_ ξ ξ =

T

D m

D( ) ( )]

[ ξ_{1 } ξ

= D

Erwartungswerte:

Varianzen:

Kovarianzmatrix:

















=

mm m

m

m m

σ σ

σ

σ σ

σ

σ σ

σ















2 1

2 22

21

1 12

11

Σ

)]

( ) [(

) ,

cov(

_{i j i i j j}

j

i

ξ ξ ξ ξ ξ ξ

σ = = Ε − −

d. h.

) ( ]

) [(

) ,

cov(

_{i i i i} ² _i

i

i

ξ ξ ξ ξ D ξ

σ = = Ε − =

wobei

i j j

i

σ

σ =

⁽ Σ ist symmetrisch)

Korrelation zwischen Zufallsvariablen:

Korrelationskoeffizienten:

[ ]

[ ^{( ( )}

²

⁾⁽ ] [ ^{( ) )}

²

]

) (

) (

) ,

cov(

j j

i i

j j

i i

j i

j i

j

i

D D

r ξ ξ ξ ξ

ξ ξ

ξ ξ

ξ ξ

ξ ξ

− Ε

− Ε

−

−

= Ε

=

= 1

i

r

i

− 1 < r

_{i j}

< 1 für i ≠ j 0

) ,

cov( ξ

_i

ξ

_j

=

Es gibt die folgenden Beziehungen:

und

• Wenn , bedeutet , haben keine Korrelation.

= 0

j

r

i

ξ

_i

, ξ

_j

• Wenn bedeutet, wenn , sehr wahrscheinlich

r

_{i j}

> 0 , ξ

_i

> ξ

_i

j

.

j

ξ

ξ >

Korrelation zwischen Zufallsvariablen:

Standardform der Variablen: i m

D _i

i i

i

S 1, ,

) (

) (

, −Ε = 

= ξ

ξ ξ ξ

Dann ist die Kovarianzmatrix:

 

 





 

 





=

1 1

1

2 1

2 21

1 12















m m

m m

S

r r

r r

r r

Σ

Wenn es keine Korrelation besteht, ist die Kovarianzmatrix eine diagonale Matrix.

Multivariate Normalverteilung:

 

  − − −

= ( )

⁻

( )

2 exp 1

) det(

) 2 ( ) 1

, ,

(

₁

ξ μ Σ

¹

ξ μ

Σ

T m m

ξ π ξ

ρ _



 



= 

2 2 2

1 12

2 1 12 2

1

σ σ

σ

σ σ σ

r Σ r

∫ ∫

∞

− −∞

− 

− − −

= ^{1 1} ₁

1 ( ) ( )

2 exp 1

) det(

) 2 ( ) 1

, , (

z z

m T

m m

m

d d

z z

F ξ ξ

π ^{ }

 ξ μ Σ ξ μ

Σ

]T

[

µ

₁

µ

₂ μ =

Für zwei Zufallsvariablen :

ξ

₁

, ξ

₂

Dichtefunktion:

Wahrscheinlichkeitsfunktion:

) ,

(

~ μ Σ

ξ N

Simulation mehrerer normalverteilten Zufallsvariablen



 



 



 



 



 



= 











 



 



 



 



 



 





400 140

140 , 100

400 , 300

~ ₂

2 2

1 12

2 1 12 2

1 2

1 2

1 N

r N r

σ σ

σ

σ σ σ

µ µ ξ

ξ

260 280 300 320 340

320 360 400 440 480

r=0,7

260 280 300 320 340

320 360 400 440 480

r=-0,7

Ein Beispiel:

) ,

(

~ μ Σ

ξ N

) ,

(

~ N A μ b A Σ A

^T

η +

b A ξ

η = +

Multivariate Normalverteilung:

Lineare Transformation:

Stochastische Prozesse:

) ξ (t

Zeitabhängige Zufallsvariable:

• An jedem Zeitpunkt ist der Wert der Variable unsicher.

• Es gibt einen zeitlichen Verlauf von Erwartungswerten.

• Zwischen Zeitpunkten gibt es Korrelationen.

Durch eine lineare Transformation ergibt sich wieder eine Normalverteilung.

Simulation stochastischer Prozesse: ξ ^{(t )} )

ξ (t

wird durch Unterteilung der Zeit mit Zeitintervallen diskretisiert.

In jedem Intervall wird sie mit einer Zufallsgröße angenähert.

t) [ ]T

(

ξ

₁

ξ

₆₀

ξ

= _{ ξ ~ N(μ, Σ)}

Z.B. und

0 10 20 30 40 50 60

220 260 300 340 380

) 60

( , , 1

05 , 0 1 )

, (

20 )

(

60 , , 1 ,

) 5 , 0 60

/ ( 200 320

)

( ²

k i

i i

k k r

k

k k

k

−

=

−

= +

=

=

−

−

=





σ

µ

• werden gemessen.

• Störgrößen sind unbekannt.

• Zustandsgrößen sind abzuleiten.

x

_k

y

k

k k

v w ,

Auslegung eines Beobachters:

Zustandsraumdarstellung diskreter Systeme

Strukturbild:

Prozess

Kalman-Filter

Die Varianz des Schätzfehlers der Zustandsgrößen soll minimiert werden !

n k

k k

k

= Ax

₋

+ Bu + w x w ∈ ℜ

x

₁

,

m k

k

k

= Cx + v y v ∈ ℜ

y ,

) ,

(

~ 0 Q

w

_k

N

) ,

(

~ 0 R

v

_k

N

Unsichere (stochastische) Störungen:

Modellgleichungen:

Die Schätzung des Zustands

x

_k anhand der Messung :

y

_k

k

x ˆ

k

(Erwartungswert auf Basis vom Modell):

1 1

1 1 1

ˆ ˆ

ˆ ˆ

−

−

−

−

−

=

+

=

k k k

k

k k k k

k

x C y

Bu x

A x

Schätzfehler der Zustandsvariablen (Kovarianzmatrix der Schätzfehler):

k T

k k k

T T

k k k

k k k

T k k T

k k k

k k k

T k k

k k k k

k k

T k k k

k k k k

k

T k k

k k k

k k k

k k k

k

T k k k

k k k

k k k

k

T k k k

k k k

k k k

k k

Q A

AΣ Q

A x

x x

x A

w w x

x A x

x A

w x

x A w

x x

A

x A w

Ax x

A w

Ax

Bu x

A w

Bu Ax

Bu x

A w

Bu Ax

x w

Bu Ax

x w

Bu Ax

x x

x x

x Σ x

+

= +

−

− Ε

=

Ε +

−

− Ε

=

+

− +

− Ε

=

− +

− +

Ε

=

+

− +

+ +

− +

+ Ε

=

− +

+

− +

+ Ε

=

−

− Ε

=

−

=

−

−

−

− −

−

− −

−

− −

−

− −

−

− −

−

− −

−

− −

−

− −

−

− −

−

− −

− −

− −

−

−

−

−

1 1 1

1 1 1

1 1

1 1 1

1 1 1

1 1 1

1 1 1

1 1 1

1 1 1

1 1 1

1 1 1

1 1 1 1

1 1

1 1

) ˆ )

ˆ )(

((

) (

ˆ )) (

ˆ )(

( ((

) ) ˆ )

( )(

ˆ ) (

((

) ˆ )

ˆ )(

((

) ˆ ))

( ˆ ))(

( ((

) ˆ )

ˆ )(

((

) ˆ )

ˆ )(

((

ˆ ) cov(

Achtung: Matrix B hat keinen Einfluss.

Also

Σ

_{k k−1}

= A Σ

_k−1k−1

A

^T

+ Q

_k

Schätzfehler der Ausgänge:

1

1

ˆ

~ ˆ

−

−

= −

−

=

_{k k k k k k}

k

y y y C x

y

Update der Zustandsschätzung:

ˆ ) ˆ (

ˆ ~

ˆ

_{k k}

= x

_{k k−1}

+ K

_k

y

_k

= x

_{k k−1}

+ K

_k

y

_k

− C x

_{k k−1}

x

Welche Matrix soll verwendet werden, damit die Kovarianz von Schätzfehlern minimiert wird?^k

K

Kalman-Filter

1 1

ˆ ˆ

−

− k k

k k

y

k

x

⇒ u

1 1

1

ˆ

1

−

−

−

− k k

k k

Σ

x ⇒

?

1

=

− k

k k

K

Σ y

k

⇒

k k

k k

Σ x ˆ

(Modell) (Messung)

Varianz des Fehlers der Zustandsschätzung:

T k k

T k k

k k

T k k

k T

k k k k

k

k k k

k k k

k k k

k k k

k k k

k k k

k k k k

k k k

k k k

k k

k k k

k k k k

T k k k

k k k

k k k

k k

RK K

C K

Σ I C K

I

K v

K C

K I

x x

C K

I

v K x

x C

K I

v K x

x C

K I

v K x

C K

Cx K

x x

x C v

Cx K

x x

x C y

K x

x

x x

x x

x Σ x

+

−

−

=

+

−

−

−

=

+

−

−

=

−

−

−

=

− +

−

−

=

− +

+

−

=

− +

−

=

−

− Ε

=

−

=

−

−

−

−

−

−

−

−

−

−

) (

) (

) cov(

) ˆ )(

cov(

) (

) cov(

ˆ )) )(

cov((

) ˆ )

)(

cov((

ˆ ) cov( ˆ

ˆ ))) )

ˆ ((

( cov(

ˆ ))) ˆ (

( cov(

) ˆ )

ˆ )(

((

ˆ ) cov(

1

1 1 1

1 1

1 1

1 1

T k k k T

k T k

k k

k k k

k

T k T

k k k

T k T k

k k

k k k

k

T k k

T k T k

k k T

k T k

k k

k k k

k

T k k

T k T k

k k k

k

T k k

T k k

k k k

k

K S K K

Σ C CΣ

Σ K

K R C

CΣ K

K Σ C

CΣ Σ K

RK K

K C CΣ

K K

Σ C CΣ

Σ K

RK K

K C I

CΣ Σ K

RK K

C K Σ I

C K Σ I

+

−

−

=

+ +

−

−

=

+ +

−

−

=

+

−

−

=

+

−

−

=

−

−

−

−

−

−

−

−

−

−

−

−

−

−

1 1

1

1 1

1 1

1 1

1 1

1 1

1

) (

) )(

(

) (

) (

Varianz des Fehlers der Zustandsschätzung:

Das Minimum bei

0 S

K CΣ

K

Σ = − _{k k−} ^T + _{k k} =

k k k

d

d 2( ₁) 2

Die optimale Lösung:

1 1

1 1

)

(

₋ ⁻

=

₋ ⁻

=

_{k k} ^T _{k k k} ^T _k

k

C Σ S Σ C S

K

1 1

−

= _{k k}− ^T _k

k Σ C S

K

T k T k

k T

k k k T k

k T

k k

kS K Σ C S S K Σ C K

K = ₋₁ ⁻¹ = ₋₁

1 1

1

1 1

1

)

(

₋

−

−

−

−

−

−

=

−

=

+

−

−

=

k k k

k k k

k k

T k k k T

k T k

k k

k k k

k k

k

Σ C K I

C Σ Σ K

K S K K

Σ C C Σ

Σ K Σ

Implementierung Kalman-Filter

Die optimale Matrix:

Da daher

k k k k

k Ax Bu

xˆ ₋₁ = ˆ _{−1 −1} +

k T

k k k

k AΣ A Q

Σ ₋₁ = _{−1 −1} +

ˆ

1

~ y

_k

= y

_k

− C x

_{k k}−

R C

CΣ

S_k = _{k k−1} ^T +

1 1

−

= _{k k}− ^T _k

k Σ C S

K

k k k

k k

k x K y

xˆ = ˆ ₋₁ + ~ ) 1

( − ₋

= _{k k k}

k

k I K C Σ

Σ

Prädiktion:

Update:

⇒

Implementierung Kalman-Filter

Prozess

k k k

k k

k x K y

xˆ = ˆ ₋₁ + ~ ˆ ,

ˆ_{k k 1} Ax_{k 1k 1} Bu_k

x ₋ = _{− −} + ~ ˆ ,

−1

−

= _{k k k}

k y Cx

y

Kalman- Filter

0 0 0

0

, Σ

x

Initialisierung:

Erweiterter Kalman-Filter (extended Kalman-Filter)

Die Erweiterung des Kalman-Filters auf nichtlineare Systeme!

) ,

,

(

_{k 1 k k}

k

f x u w

x =

₋

) ,

(

_{k k}

k

h x v

y =

) ,

(

~ 0 Q

w

_k

N

) ,

(

~ 0 R

v

_k

N

Unsichere (stochastische) Störungen:

Modellgleichungen:

Die Schätzung des Zustands

x

k anhand der Messung :

y

k

k

x ˆ

k

Erweiterter Kalman-Filter (extended Kalman-Filter)

Die Prädiktion der Schätzung vom letzten Schritt

(Erwartungswert auf Basis vom nichtlinearen Modell):

) ˆ ,

ˆ (

) , ˆ ,

ˆ (

1 1

1 1 1

0 x

h y

0 u

x f x

−

−

−

−

−

=

=

k k k

k

k k k k

k

Aufgrund der nichtlinearen Funktionen kann die Kovarianzmatrix nicht direkt abgleitet werden.

Daher führt man eine Linearisierung für jeden Schritt durch:

0 x

0 u x

x C h

x A f

ˆ ,

, ˆ ,

1 1

1 1

−

−

−

−

∂

= ∂

∂

= ∂

k k

k k k

k

k (Zeitvariante

Systeme!)

Implementierung Erweiterter Kalman-Filter

k T

k k k k

k

k A Σ A Q

Σ ₋₁ = _{−1 −1} +

) ˆ ,

~ (

1

0 x

h y

y

_k

=

_k

−

_{k k−}

R Σ C

C

S_k = _{k k k−1} ^T_k +

1 1

−

= _{k k}− ^T_{k k}

k Σ C S

K

k k k

k k

k x K y

xˆ = ˆ ₋₁ + ~ ) 1

( − ₋

= _{k k k k}

k

k I K C Σ

Σ

Prädiktion:

Update:

⇒

) , ˆ ,

ˆ

₁

f ( x

_{1 1}

u 0 x

_{k k−}

=

_{k− k− k}

Aufgrund der Approximation durch die Linearisierung ist die Schätzung der Kovarianz nicht genau.

Daher wird das Ergebnis oft nicht zufrieden stellend.