Note de curs - Departamentul Automatica, Calculatoare si ...

T
T
− 1
P( t) = M( t) − M( t) H ( t)[ H( t) M( t) H ( t) + Q ( t)] H( t) M( t)
M( t) =
T
F( t − 1) P( t − 1) F ( t − 1) + Qw
( t − 1)
M( 0)
= M0
z( 0)
= z0
cu H( t)
= ∂ h( t, z( t))
, M( t) =
∂ z
T
E{( z( t) − z( t))( z( t) − z( t)) } o matrice de
covariatie evaluatǎ înainte de mǎsurǎtorile de la momentul t, K(t) amplificarea
Kalman, Q v (t) matricea de covariatie a mǎsurǎtorilor si z( t) estimarea lui z(t)
înaintea masurǎtorii de la momentul t. Matricile F(t) sunt definite prin
submatricile lor F ij (i, j = 1, 2, 3):
∂ f d
∂ f d
∂ f
d
F11 = F12 = F13
=
∂ x
∂ x
∂ θ
d
− 1
⎛ ∂ f ⎞
s
∂ f
s
∂ fˆ
d
F21
= −
⎜
∂ x
⎟
⎝ s ⎠ ∂ xd
∂ xd
− 1
⎛ ∂ f ⎞
s
∂ f
s
∂ fˆ
d
F22
= −
⎜
∂ x
⎟
⎝ s ⎠ ∂ xd
∂ xs
− 1
⎛ ∂ f ⎞ ⎛
⎞
s ⎜
∂ f
s
∂ fˆ
d
∂ f
s
F = −
+ ⎟
⎜
⎟
23
⎝ ∂ xs
⎠ ⎝ ∂ xd
∂ θ ∂ θ ⎠
F 31 = 0 F 32 = 0 F 33 = I
De asemenea, matricea Q w (t) este definitǎ prin submatricile ei Q ij (i, j = 1, 2, 3):
Q
22
⎛ ∂ f ⎞ ⎛
s
∂ f ⎞
s
Q11 = Qd
Q12 = − Qd
⎜ ⎟ ⎜ ⎟ Q13 = 0
⎝ ∂ x ⎠ ⎝ ∂ x ⎠
⎛
Q21
= −
⎜
⎝
1
f
s
f
s
= ⎛ −
Qs
Qd
⎝ ⎜ ∂ ⎞ ⎡
⎟ + ⎛ xs
⎠
⎝ ⎜ ∂ ⎞ ⎛
⎢
⎟ ⎜
∂
x
⎣⎢
∂
d ⎠ ⎝
În expresiile de mai sus
∂
∂
Q
23
⎛
= −
⎜
⎝
∂ f
s
∂ x
∂
∂
∂
∂
s
f
x
f
x
s
s
⎞
⎟
⎠
s
d
d
− 1
s
T
∂ f
s
∂ x
T
d
s
Q
d
− T
⎞ f
s
f
s
⎟ + ⎛ Q
⎠ ⎝ ⎜ ∂ ⎞ ⎛ ∂ ⎞
⎟ θ ⎜ ⎟
∂ θ ⎠ ⎝ ∂ θ ⎠
⎞
⎟
⎠
⎛ ∂ f ⎞
Q = 0 Q = − Qθ
⎜ ⎟
⎝ ∂ θ
− 1
∂ f
s
∂ θ
⎛ ∂ f
⎜
⎝ ∂ x
Q
θ
v
⎞
⎟ Q = Q
⎠
s s
31 32 33
⎠ s
f
s
x
T
− T
∂ f
∂ f
s
d
∂ f
d
= =
∂ z
∂ z ∂ z
z= z ( t + 1) z=
z( t )
θ
T
⎤ ⎛
⎥ ⎜
⎦⎥
⎝
∂
∂
f ⎞
s
⎟
x ⎠
s
− T
82