Page 126 - Autonomous Mobile Robots

Data Fusion via Kalman Filter
hh
109
Tuning. Ideally, the KF is applied to a well-modeled dynamic system with
stochastic process noise and measurement noise satisfying the required assump-
tions. In such cases, the Q and R matrices can be computed correctly as a portion
of the stochastic model. In some other applications, examples of which will
occur in Section 3.3.3, the vector ω represents unknown factors that may not
be truly random. In such applications, Q and R are often used as performance
tuning parameters. As Q is decreased relative to R, the KF trusts the dynamic
model of the system more than the measurements; therefore, the states of the
system converge more slowly since new information is weighted less. If Q is
increased relative to R, the measurements will be weighted more and the states
will converge faster; however, the measurement noise will have a larger effect
on the accuracy of the filtered solution. Note that in applications where Q and
R are used as performance tuning parameters, all stochastic interpretations of
P k|k are lost. Instead, the KF is being used as an algorithm to estimate the state,
but the KF optimality properties are not applicable.
Maintaining symmetry. The equation
P k|k =[I − K k H k ]P k|k−1 (3.32)
is a simplified version of
T
P k|k =[I − K k H k ]P k|k−1 [I − K k H k ] + K k R k K T (3.33)
k
Equation (3.32) is valid only when K k is the optimal Kalman gain matrix.
When K k is defined by an equation other than Equation (3.23) and is not the KF
optimal gain matrix, then Equation (3.33) should be used. Since P k|k is the error
covariance matrix, it should be symmetric and positive semidefinite. Although
Equation (3.33) requires more computational operations than Equation (3.32)
does, Equation (3.33) is a symmetric equation. However, the symmetry of either
result can be guaranteed and the computational requirements are decreased by
only computing the lower diagonal half of P k|k .
Maintaining definiteness. Neither Equation (3.32) nor Equation (3.33)
guarantees that P k|k is symmetric or positive semidefinite in the presence of
T
numeric errors. One possible solution is to factorize P k|k (e.g., P = UDU or
P = QR) and derive algorithms that propagate the factors directly. Such fac-
torized algorithms [20,21] have better numeric stability properties, especially
in applications where computational error is an issue.
3.2.3 Extended KF
The previous sections have discussed only linear systems with zero-mean, white
Gaussian process, and measurement noise. The optimality properties of the KF
© 2006 by Taylor & Francis Group, LLC
FRANKL: “dk6033_c003” — 2006/3/31 — 16:42 — page 109 — #11