208
C. Greco and M. Vasile
apply the Kalman filter to the corresponding linearised system. Many filters have
been developed following the linearisation procedure [35]. In this section, one
approach based on the first-order truncation of the Taylor series (see Sec. 6.2.2.1)
will be presented. This technique extends the Kalman filter application to filtering
problems with differentiable nonlinear functions, and therefore it is named extended
Kalman filter [21, 43, 56]. It is clear that this method will fail if the true state is not
close enough to the reference point of the expansion, i.e. when the nonlinear terms
cannot be reasonably neglected.
The nonlinear filtering problem considered here is defined by the following
model:
˙
x = f(t, x) + G(t)w
y = h(t, x) +
x 0 ∼ N x 0 (ˆ x 0 , P 0 ) .
(6.73)
The nonlinear transformations f(t, x) and h(t, x) can be expanded around the
reference trajectory generated by the deterministic term in the equations of motion
with initial condition ¯
x k :
˙ ¯
x(t) = f(t, ¯
x)
¯
x(t 0 ) = ¯
x 0 .
(6.74)
Therefore, the equations for the deviation δx = x − ¯
x evolution from the reference
trajectory can be approximated as:
δ ˙
x = f(t, x) − f(t, ¯
x) + G(t)w
= f(t, ¯
x) − f(t, ¯
x) + ∇ x f
¯
x
δx + O
δx
2
+ G(t)w
≈ ∇ x f
¯
x
δx + G(t)w .
(6.75)
Similarly, the observations can be defined as deviation δy = y− ¯
y with respect to the
deterministic measurements that would result from ¯
x. The resulting approximated
model follows as:
δy = h(t, x) − h(t, ¯
x) +
= h(t, ¯
x) − h(t, ¯
x) + ∇ x h
¯
x
δx + O
δx
2
+
≈ ∇ x h
¯
x
δx + .
(6.76)
For both the linearised dynamics and observation model, the partial derivative
Jacobian matrix is evaluated along the reference trajectory ¯
x. The prior distribution
of the corresponding linear system follows from the linearity of the expectation
operator and the fixed deterministic nature of the initial reference state:
C. Greco and M. Vasile
apply the Kalman filter to the corresponding linearised system. Many filters have
been developed following the linearisation procedure [35]. In this section, one
approach based on the first-order truncation of the Taylor series (see Sec. 6.2.2.1)
will be presented. This technique extends the Kalman filter application to filtering
problems with differentiable nonlinear functions, and therefore it is named extended
Kalman filter [21, 43, 56]. It is clear that this method will fail if the true state is not
close enough to the reference point of the expansion, i.e. when the nonlinear terms
cannot be reasonably neglected.
The nonlinear filtering problem considered here is defined by the following
model:
˙
x = f(t, x) + G(t)w
y = h(t, x) +
x 0 ∼ N x 0 (ˆ x 0 , P 0 ) .
(6.73)
The nonlinear transformations f(t, x) and h(t, x) can be expanded around the
reference trajectory generated by the deterministic term in the equations of motion
with initial condition ¯
x k :
˙ ¯
x(t) = f(t, ¯
x)
¯
x(t 0 ) = ¯
x 0 .
(6.74)
Therefore, the equations for the deviation δx = x − ¯
x evolution from the reference
trajectory can be approximated as:
δ ˙
x = f(t, x) − f(t, ¯
x) + G(t)w
= f(t, ¯
x) − f(t, ¯
x) + ∇ x f
¯
x
δx + O
δx
2
+ G(t)w
≈ ∇ x f
¯
x
δx + G(t)w .
(6.75)
Similarly, the observations can be defined as deviation δy = y− ¯
y with respect to the
deterministic measurements that would result from ¯
x. The resulting approximated
model follows as:
δy = h(t, x) − h(t, ¯
x) +
= h(t, ¯
x) − h(t, ¯
x) + ∇ x h
¯
x
δx + O
δx
2
+
≈ ∇ x h
¯
x
δx + .
(6.76)
For both the linearised dynamics and observation model, the partial derivative
Jacobian matrix is evaluated along the reference trajectory ¯
x. The prior distribution
of the corresponding linear system follows from the linearity of the expectation
operator and the fixed deterministic nature of the initial reference state: