6 Fundamentals of Filtering
205
than the state dimension. Furthermore, if the observations are uncorrelated, i.e. if R k
is diagonal, the observations can be processed one by one, therefore requiring only
a scalar division, eventually leading to the same estimate obtained by processing
the batch ¯
y k at once [29]. Therefore, for observations the algorithm can now be
schematised as in Algorithm 1. The Kalman Filter is one of the very few closedAlgorithm 1 Kalman Filter
Given the filtering model in Eq. (6.57)
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:
Propagate mean with ˙ ˆ
x = F ˆ
x
ˆ
x
+
k−1 → ˆ
x
−
k
4:
Propagate covariance with ˙
P = F P x + P x F T + GQG T
P
+
k−1 → P
−
k
Update step: after observation ¯
y k compute p(x k |y 1:k ) = N x k (ˆ x
+
k , P
+
k )
5:
Compute Kalman gain
K k = P
−
k H T (H P
−
k H T + R k ) −1
6:
Update mean with observation information
ˆ
x
+
k = ˆ
x
−
k + K k (¯ y k − H ˆ
x
−
k )
7:
Update covariance with observation covariance
P
+
k = P
−
k − K k H P
−
k
8:
Update quantities for loop iteration
ˆ
x
+
k−1 = ˆ
x
+
k , P
+
k−1 = P
+
k , k = k + 1
9: end for
form solutions of the general sequential filtering equations. Although it relies on
restrictive assumptions, historically it had a crucial role in the development of
approximated numerical techniques which are employed in real-world applications.
This version of the Kalman filter is likely the most simple for gaining insight in
its prediction-update sequential scheme. However, for actual numerical implementation, Algorithm 1 is not the most robust alternative. Indeed, numerical errors
could cause the covariance matrix to lose its symmetry and positive definiteness
properties [58]. Rather than Eq. (6.66), alternative updates for the covariance matrix
are proposed to have a better numerical stability [4, 7, 9, 58] . For a specialised and
instructive overview on the practical algorithm variants for the Kalman filter, the
reader is reminded to specific references [25].
One formulation that is worth to discuss is the one employing the state transition
matrix to propagate the state. Indeed, as anticipated in the previous section, the
dynamics of a linear system can be formulated by integral equations as [29]:
x k = Φ(t k , t k−1 )x k−1 +
t k
t k−1
Φ(t k , τ )G(τ )dw .
(6.68)
205
than the state dimension. Furthermore, if the observations are uncorrelated, i.e. if R k
is diagonal, the observations can be processed one by one, therefore requiring only
a scalar division, eventually leading to the same estimate obtained by processing
the batch ¯
y k at once [29]. Therefore, for observations the algorithm can now be
schematised as in Algorithm 1. The Kalman Filter is one of the very few closedAlgorithm 1 Kalman Filter
Given the filtering model in Eq. (6.57)
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:
Propagate mean with ˙ ˆ
x = F ˆ
x
ˆ
x
+
k−1 → ˆ
x
−
k
4:
Propagate covariance with ˙
P = F P x + P x F T + GQG T
P
+
k−1 → P
−
k
Update step: after observation ¯
y k compute p(x k |y 1:k ) = N x k (ˆ x
+
k , P
+
k )
5:
Compute Kalman gain
K k = P
−
k H T (H P
−
k H T + R k ) −1
6:
Update mean with observation information
ˆ
x
+
k = ˆ
x
−
k + K k (¯ y k − H ˆ
x
−
k )
7:
Update covariance with observation covariance
P
+
k = P
−
k − K k H P
−
k
8:
Update quantities for loop iteration
ˆ
x
+
k−1 = ˆ
x
+
k , P
+
k−1 = P
+
k , k = k + 1
9: end for
form solutions of the general sequential filtering equations. Although it relies on
restrictive assumptions, historically it had a crucial role in the development of
approximated numerical techniques which are employed in real-world applications.
This version of the Kalman filter is likely the most simple for gaining insight in
its prediction-update sequential scheme. However, for actual numerical implementation, Algorithm 1 is not the most robust alternative. Indeed, numerical errors
could cause the covariance matrix to lose its symmetry and positive definiteness
properties [58]. Rather than Eq. (6.66), alternative updates for the covariance matrix
are proposed to have a better numerical stability [4, 7, 9, 58] . For a specialised and
instructive overview on the practical algorithm variants for the Kalman filter, the
reader is reminded to specific references [25].
One formulation that is worth to discuss is the one employing the state transition
matrix to propagate the state. Indeed, as anticipated in the previous section, the
dynamics of a linear system can be formulated by integral equations as [29]:
x k = Φ(t k , t k−1 )x k−1 +
t k
t k−1
Φ(t k , τ )G(τ )dw .
(6.68)