State estimation with Kalman Filter

F. Haugen: Kompendium for Kyb. 2 ved Høgskolen i Oslo 111
• Auto-covariance matrix (for lag zero) of the estimation error of the
predicted estimate:
n
o
P d = R exd (0) = E (x − m xd )(x − m xd ) T (8.40)
• The transition matrix A of a linearized model of the original
nonlinear model (8.14) calculated with the most recent state
estimate, which is assumed to be the corrected estimate x c (k):
A = ∂f(·)
¯
∂x
¯xc (k),u(k)
(8.41)
• The measurement gain matrix C of a linearized model of the original
nonlinear model (8.15) calculated with the most recent state
estimate:
C = ∂g(·)
¯
(8.42)
∂x
¯xc(k),u(k)
However, it is very common that C = g(), and in these cases no
linearization is necessary.
The Kalman Filter gain is calculated as follows (these calculations are
repeated each program cycle):
Kalman Filter gain:
1. This step is the initial step, and the operations here are executed
only once. The initial value P p (0) can be set to some guessed value
(matrix), e.g. to the identity matrix (of proper dimension).
2. Calculation of the Kalman Gain:
Kalman Filter gain:
K(k) =P p (k)C T [CP p (k)C T + R] −1 (8.43)
3. Calculation of auto-covariance of corrected state estimate error:
Auto-covariance of corrected state estimate error:
P c (k) =[I − K(k)C] P p (k) (8.44)
4. Calculation of auto-covariance of the next time step of predicted state
estimate error:
Auto-covariance of predicted state estimate error:
P p (k +1)=AP c (k)A T + GQG T (8.45)