214
C. Greco and M. Vasile
ˆ
z =
g(x)N x (ˆ x, P x )dx
S =
(g(x) − ˆ
z)(g(x) − ˆ
z)
T
N x (ˆ x, P x )dx
C =
(x − ˆ
x)(g(x) − ˆ
z)
T
N x (ˆ x, P x )dx .
(6.84)
From the moment matching formulation, a Gaussian can be fit to all the
probability densities involved in the prediction and update step by using the
definition of expectation. By defining the integral equations of motion as
x(t k ) = F(x k−1 )
t k
t k−1
f(t, x)dt + x(t k−1 ).
(6.85)
the general algorithm for the Gaussian filter is schematised in Algorithm 5, using
the same form of update equations as presented for the UKF [51].
Algorithm 5 Gaussian filter
Given the filtering model in Eq. (6.73)
1: Initialise t k−1 = t 0 , ˆ
x
+
k−1 = ˆ
x 0 , P
+
k−1 = P 0 , t k = t 1
2: for Observation times do
Prediction step: compute p(x k |y 1:k−1 ) = N x k (ˆ x
−
k , P
−
k )
3:
Compute mean estimate
ˆ
x
−
k =
F(x k−1 )N (ˆ x
+
k−1 , P
+
k−1 )dx k−1
4:
Propagate covariance matrix
P
−
k =
(F(x k−1 ) − ˆ
x
−
k )(F(x k−1 ) − ˆ
x
−
k ) T N (ˆ x
+
k−1 , P
+
k−1 )dx k−1
Update step: after observation ¯
y k compute p(x k |y 1:k ) = N x k (ˆ x
+
k , P
+
k )
5:
Compute predicted observation mean, covariance and state observation cross covariances
ˆ
y k =
h(t k , x k )N (ˆ x
−
k , P
−
k )dx k
S k =
(h(t k , x k ) − ˆ
y k )(h(t k , x k ) − ˆ
y k ) T N (ˆ x
−
k , P
−
k )dx k
C k =
(x k − ˆ
x
−
k )(h(t k , x k ) − ˆ
y k ) T N (ˆ x
−
k , P
−
k )dx k
6:
Compute Kalman gain
K k = C k S
−1
k
7:
Update mean with observation information
ˆ
x
+
k = ˆ
x
−
k + K k (¯ y k − ˆ
y k )
8:
Update covariance with observation covariance
P
+
k = P
−
k − K k S k K T
k
9:
Update quantities for loop iteration
ˆ
x
+
k−1 = ˆ
x
+
k , P
+
k−1 = P
+
k , k = k + 1
10: end for
All the methods developed previously can be rederived from this general form
depending on how the integrals above are solved. Indeed, this is the generalisation
for general nonlinear functions of Eq. (6.60), used to derive the KF. The EKF is
obtained by approximating g(x) at the first order in Eq. (6.84), which then can be