Initialization |
Initialize Ȃ0 and
, λ, R0
|
Forward pass |
1. For n from 1 to N
|
2. Construct the data history matrix Hn
|
3. Compute: one step prediction of observation:
|
4. Compute residuals: rn = xn − x̑n
|
5. Update the covariance matrix:
, and compute
according to Eq. (A8)
|
6. Compute Kalman Gain:
|
7. Compute state estimate: Ȃn = Ȃn−1 + Knen
|
8. Compute estimated state covariance:
|
9. Update state noise covariance: Wn = λPn
|
10. Calculate a priori estimated state covariance:
|
END |
Backward pass |
For n from N − 1 to 1 |
Let
|
1. Compute smoothed estimated state covariance:
|
2. Compute smoothed state estimate:
|
END |
Note, the initial smoothed estimate at step N for the backward pass is the final state estimate for the forward pass:
|
Similarly,
|