EKF - How to detect if filter is converged?

Question

I implemented an EKF. The algorithm works very well but I need a criterion to detected when the filter is converged after initialisation. What is the best / most common way to do this. I have two ideas:

1.) When innovation has reached a pre defined limit. 2.) When estimated variance has reached a pre defined limit.

Any suggestions ?

Answer 1

The most common error when dealing with Kalman filters (and especially EKF) is to think that the convergence of the P matrix is equivalent to real convergence of the estimation.

You need to look at the normalized innovations.

The innovation is the difference between the expected (predicted measurement) to the actual one: Innov = y - h(x_predicted)

The normalized innovation is the Mahalanobis distance of the innovation, which correlates with the convergence once the P matrix is small:

d^2 = Innov.transpose * Cov(Innov).inverse * Innov

Where Cov(Innov) = Cov(y - h(x_predicted)) = R + H * P_predicted * H.Transpose

Answer 2

I usually look at the derivative of the estimated variance. If it is not changing any more, the filter has converged. This usually works quite well.