8
1
8
Kalman Filtersas Dynamic SystemState Observers
8.1The DiscreteTime Linear Kalman Filter.................
8

1
Linearization of Dynamic and Measurement System Models
Linear Kalman Filter Error Covariance Propagation
ã
Linear Kalman Filter Update
8.2Other Kalman Filter Formulations...........................
8

6
The Continuous–Discrete Linear Kalman Filter
ã
The Continuous–Discrete Extended Kalman Filter
8.3Formulation Summary and Review.........................
8

88.4Implementation Considerations...............................
8

9References.................................................................................
8

10
8.1The DiscreteTime Linear Kalman Filter
Distilled to its most fundamental elements, the Kalman filter [1] is a predictor–corrector estimationalgorithm that uses a dynamic system model to predict state values and a measurement model to correctthis prediction. However, the Kalman filter is capable of a great deal more than just state observation insuch a manner. By making certain stochastic assumptions, the Kalman filter carries along an internal metricof the statistical confidence of the estimate of individual state elements in the form of a covariance matrix.The essential properties of the Kalman filter are derived from the requirements that the state estimate be
ã
Linear combination of the previous state estimate and current measurement information
ã
Unbiased with respect to the true state
ã
Optimal in terms of having minimum variance with respect to the true stateStarting with these basic requirements an elegant and efficient formulation for the implementation of the Kalman filter may be derived.The Kalman filter processes a time series of measurements to update the estimate of the system stateand utilizes a dynamic model to propagate the state estimate between measurements. The observedmeasurement is assumed to be a function of the system state and can be represented via
(8.1)
where
Y
(
t
) is an
m
dimensional observable,
h
is the nonlinear measurement model,
X
(
t
) is the
n
dimensional system state,
β
is a vector of modeling parameters, and
v
(
t
) is a random process accountingfor measurement noise.
Y
t
( )
h
X
t
( )
,
β
,
t
( )
v
t
( )+=
Timothy P. Crain II
NASA Johnson Space Center
© 2008 by Taylor & Francis Group, LLC
8
2
Mechatronic System Control, Logic, and Data Acquisition
The true dynamic system is described by a general firstorder, ordinary differential equation
(8.2)
where
f
is the nonlinear dynamics function that incorporates all significant deterministic effects of theenvironment,
α
is a vector of parameters used in the model, and
w
(
t
) is a random process that accountsfor the noise present from mismodeling in
f
or from the quantum uncertainty of the universe, dependingon the accuracy of the deterministic model in use.With these general models available, a linear Kalman filter (LKF) may be derived in a discretetimeformulation. The dynamics and measurement functions are linearized about a known reference state,(
t
), which is related to the true environment state,
X
(
t
), via
(8.3)
The LKF state estimate is related to the true difference by
(8.4)
where the “” denotes the state estimate (or filter state), is the estimation error, and “
±
” indicateswhether the estimate and error are evaluated instantaneously before (
−
) or after (
+
) measurement updateat discrete time
t
k
.It is important to emphasize that the LKF filter state is the estimate of the difference between theenvironment and the reference state. The LKF mode of operation will therefore carry along a referencestate and the filter state between measurement updates. Only the filter state is at the time of measurementupdate. Figure 8.1 illustrates the generalized relationship between the true, reference, and filter states inan LKF estimating a twodimensional trajectory.
8.1.1Linearization of Dynamic and Measurement System Models
The dynamics and measurement functions may be linearized about the known reference state, (
t
),according to
(8.5)(8.6)
FIGURE 8.1
LKF tracking of a twodimensionaltrajectory.
Reference TrajectoryTrue TrajectoryState EstimateEstimation Error
X
·
t
( )
f
X
t
( )
,
α
,
t
( )
w
t
( )+=
XX
˜
t
( )
x
t
( )+
X
t
( )=
x
ˆ
k
±( )
x
k
δ
x
k
±( )
+=
ˆ
δ
x
k
±( )
X
˜
f
X
,
α
,
t
( )
.
f
X
˜
t
( )
,
α
,
t
( )
F X
˜
t
( )
,
α
,
t
( )
x
t
( )
w
t
( )+ +
h
X
,
α
,
t
( )
.
h
X
˜
t
( )
,
β
,
t
( )
H X
˜
t
( )
,
β
,
t
( )
x
t
( )
v
t
( )+ +
© 2008 by Taylor & Francis Group, LLC
Kalman Filters as Dynamic System State Observers
8
3
where
F
is the dynamics partial derivative matrix and
H
is the measurement partial derivative matrixdefined by
(8.7)(8.8)
and
x
(
t
) is the true state to be estimated representing the difference between the environment and referencestates
(8.9)
After linearizing the dynamic and measurement models, the effect of neglecting the higher order termsis assumed to be included in the random processes
w
(
t
) and
v
(
t
). The linearization is an acceptableapproximation if
x
(
t
) is sufficiently small.The reference and filter states are propagated according to the discretetime linear relationship
(8.10)(8.11)
where
Φ
(
t
k
+
1
,
t
k
) is the state transition matrix from time
t
k
to time
t
k
+
1
and has the following properties:
(8.12)
Note that the system dynamics are now incorporated into the propagation of the reference and filterstates by the integration of the dynamics partial derivative in Equation 8.13.Mathematically, the true difference state is propagated in a similar fashion with the addition of aprocess noise random value
(8.13)
In general, it is not required that the reference dynamic model be exactly the same as the truth dynamicsor that the modeling parameter
α
be equivalent to the true modeling vector. This notation is left in placeto simplify the derivation of the Kalman filter formulation. A number of innovative approaches have beendeveloped for adapting reference model parameters to improve fidelity with the unknown realworldsystem model [2–6] and can be used to enhance filter operation.The LKF also requires a linearized measurement,
y
k
=
Y
k
−
, modeled by
(8.14)
For the development of the Kalman filters presented here, the random contributions
v
k
and
w
k
are assumedto be discrete realizations of the continuous zero mean Gaussian process in Equations 8.1 and 8.2 andare defined by
(8.15)(8.16)
F X
˜
t
( )
,
α
,
t
( )
∂
f
∂
X

X
=
X
˜
=
H X
˜
t
( )
,
β
,
t
( )
∂
h
∂
X

X
=
X
˜
=
x
t
( )
X
t
( )
X
˜
t
( )
–=
X
˜
k
+
1
Φ
t
k
+1
,
t
k
( )
X
˜
k
=
x
ˆ
k
+
1
−( )
Φ
t
k
+1
,
t
k
( )
x
ˆ
k
−( )
=
Φ
t
k
,
t
k
( )
I
=Φ
·
t
k
+1
,
t
k
( )
F X
˜
t
( )
,
α
,
t
( )Φ
t
k
+1
,
t
k
( )=Φ
t
k
+2
,
t
k
( ) Φ
t
k
+2
,
t
k
+1
( )Φ
t
k
+1
,
t
k
( )=
x
k
+
1
Φ
t
k
+1
,
t
k
( )
x
k
w
k
+=
Y
˜
k
y
k
H
k
x
k
v
k
+=
E
v
k
v
i T
R
k
δ
ki
=
E
w
k
w
i T
Q
k
δ
ki
=
© 2008 by Taylor & Francis Group, LLC
8
4
Mechatronic System Control, Logic, and Data Acquisition
Generally, it is assumed that the process noise and measurement noise sequences are uncorrelated so that
(8.17)
However, the Kalman filter can be configured to operate in systems where this assumption does not apply [7].
8.1.2Linear Kalman Filter Error Covariance Propagation
The propagation of the filter and reference states in the LKF were outlined in the previous section inEquations 8.11 and 8.13. However, all Kalman filter formulations must also propagate a confidence metricof the state estimate in the form of a state error covariance matrix. The state error covariance,
P
, is definedas the expectation of the outer product of the estimation error vector
(8.18)
The state error covariance matrix is
n
×
n
and symmetric, and must remain positive definite to retain filterstability. The mechanism for propagating the covariance can be derived by taking the covariance just beforemeasurement update at time
t
k
+
1
(8.19)
and substituting the estimation error and propagation definitions in Equations 8.4, 8.11, and 8.13 to yield
(8.20)
Utilizing the definitions of process noise covariance in Equation 8.16 and state error covariance inEquation 8.18 the propagation equation reduces to
(8.21)
The propagation equation can be interpreted as the sum of the mapping of the previous postupdateerror covariance through the system dynamics and the system process noise induced uncertainty. Thus,process noise acts to increase the state error covariance between measurement updates.
8.1.3Linear Kalman Filter Update
The LKF seeks an unbiased, minimum variance solution for the difference state,
x
k
, by combining previousstate information with available measurements. The state estimate after measurement update is thereforeassumed to be a linear combination of the preupdate state and the linearized measurement information
(8.22)
Substituting Equations 8.4 and 8.14 into Equation 8.22 and solving for the estimation error yields
(8.23)
E
w
k
v
i T
0
k
,
i
∀∀=
P
k
±( )
E
δ
x
k
±
δ
x
k
±
T
=
P
k
+
1
−( )
E
δ
x
k
+
1
−( )
δ
x
k
+
1
−( )
T
=
P
k
+
1
−( )
E
Φ
t
k
+1
,
t
k
( )
x
ˆ
k
+( )
x
k
–
x
ˆ
k
+( )
x
k
–
T
Φ
t
k
+1
,
t
k
( )
T
w
k
w
k T
+
=
P
k
+
1
−( )
Φ
t
k
+1
,
t
k
( )
P
k
+( )
Φ
t
k
+1
,
t
k
( )
T
Q
k
+=
x
ˆ
k
+( )
K
k
∗
x
ˆ
k
−( )
K
k
z
k
+=
δ
x
k
+( )
K
k
∗
K
k
H
k
I
–
+( )
x
k
K
+
k
∗
δ
x
k
−( )
K
k
v
k
+=
© 2008 by Taylor & Francis Group, LLC