Information Technology Reference
In-Depth Information
x.k C 1/ D A.k/x.k/ C B.k/
u
.k/ C
w
.k/
z
.k/ D
Cx
.k/ C
v
.k/
(4.87)
where the state x.k/ is a m-vector,
w
.k/ is an m-element process noise vector,
and A is an m m real matrix. Moreover the output measurement
z
.k/ is a p-
vector, C is a pm-matrix of real numbers, and
v
.k/ is the measurement noise.
It is assumed that the process noise
w
.k/ and the measurement noise
v
.k/ are
uncorrelated. The process and measurement noise covariance matrices are denoted
as Q.k/ and R.k/, respectively. Now the problem is to estimate the state x.k/based
on the measurements
z
.1/;
z
.2/; ;
z
.k/. This can be done with the use of Kalman
Filtering. The discrete-time Kalman filter can be decomposed into two parts: (1)
time update (prediction stage), and (2) measurement update (correction stage).
Measurement update
:
K.k/ D P
.k/C
T
ŒCP
.k/C
T
C R
1
x.k/ Dx
.k/ C K.k/Œ
z
.k/ C x
.k/
P.k/D P
.k/ K.k/
CP
.k/
(4.88)
Time update
:
P
.k C 1/ D A.k/P.k/A
T
.k/ C Q.k/
x
.k C 1/ D A.k/x.k/ C B.k/
u
.k/
(4.89)
Next, the following nonlinear state-space model is considered:
x.k C 1/ D .x.k//C L.k/
u
.k/ C
w
.k/
z
.k/ D .x.k//C
v
.k/
(4.90)
The operators .x/ and .x/ are .x/ D Œ
1
.x/;
2
.x/; ,
m
.x/
T
, and .x/ D
Œ
1
.x/;
2
.x/; ;
p
.x/
T
, respectively. It is assumed that and are sufficiently
smooth in x so that each one has a valid series Taylor expansion. Following a
linearization procedure, about the current state vector estimate
x.k/ the linearized
version of the system is obtained:
x.k C 1/ D .x.k// C J
.x.k//Œx.k/ x.k/ C
w
.k/
z
.k/ D .x
.k// C J
.x
.k//Œx.k/ x
.k/ C
v
.k/
(4.91)
where J
.x.k// and J
.x.k// are the associated Jacobian matrices of and ,
respectively. Now, the EKF recursion is as follows [
157
].
Measurement update
. Acquire
z
.k/ and compute:
K.k/ D P
.k/J
T
.x
.k//ŒJ
.x
.k//P
.k/J
T
.x
.k// C R.k/
1
x.k/ Dx
.k/ C K.k/Œ
z
.k/ .x
.k//
P.k/D P
.k/ K.k/J
.x
.k//P
.k/
(4.92)
