Dokument 1.pdf - Universität Siegen

6.2 Parameter- und Zustandsschätzung
2. Berechnungen während des Measurement-Update (k − N +1≤ j ≤ k)
a. Filtergleichungen des EKF
ˆx + (j) = ˆx − (j)+K(j) · r(j) (6.18)
r(j) = y(j) − h[x,k]| x=ˆx
−
k
(6.19)
D(j) = I − K(j) · ˜H (6.20)
P + (j) = P − (j) − K(j) · ˜H · P − (j) = (6.21)
= D(j) · P − (j) · D(j) T − K(j) · R(j) · K(j) T (6.22)
Es werden folgende Abkürzungen zur Berechnung eingeführt:
E = P yy · r j (6.23)
F = P yy − E · E T (6.24)
b. Score-Algorithmusgleichungen
∂ˆx +
∂ϑ m
(j) = D(j) ·
[
∂ˆx
− [ ] ]
j−1
+ ∂P− · ˜HT · E
(6.25)
∂ϑ m ∂ϑ m
∂P +
(j) = D(j) · ∂P− (j) · D(j) T (6.26)
∂ϑ m ∂ϑ m
Berechnung der Summenterme des Score-Vektors für (6.31):
s m [y(j), ˆϑ]
∣ = ∂ˆx−
ˆϑ= ˆϑi
∂ϑ m
T
c. Anschließend auf laufende Summe aufaddieren
k∑
j=k−N+1
· [ ˜H T · E] − 1 2 · tr {
˜F ·
∂P yy
∂ϑ m
}
(6.27)
s m [y(j), ˆϑ]
∣ (6.28)
ˆϑ= ˆϑi
3. Nach N Rekursionen
γ m [y(k), ˆϑ]
}
∣ = tr
{P + (k) −1 · ∂P+ (k)
(6.29)
ˆϑ= ˆϑi ∂ϑ
{
m
E γ m [y(k), ˆϑ] · γ n [y(k), ˆϑ]
} ∣ = (6.30)
ˆϑ= ˆϑi
∂ˆx + T
k
· P + (k) −1 · ∂ˆx+ k
+ 1 [
]
∂ϑ m ∂ϑ n 2 · tr P + (k) −1 · ∂P+ (k) · P + (k) −1 · ∂P+ (k)
∂ϑ m ∂ϑ n
109