Frequent update of the Kalman Filter covariance matrices

Question

I have inherited some code that does vehicle localisation based on two main inputs: sensor input from the vehicle odometry and map localisation based on matching camera images to road features. Both tracking routines works reasonably well, and the outputs from both are fed into a Kalman Filter (the OpenCV one is used) with a reasonably good output under normal circumstances. However, as we round corners, we get a lot of lateral drift and lose visual tracking, so to compensate (?), we double or so the covariance for the X, Y and Yaw values for the measurementNoiseCov, as OpenCV calls it, resetting them after we get back on track; depending exactly on how we fudge the values, the match to Ground Truth improves to some degree, but I think we're just stabbing in the dark and getting lucky occasionally.

Does this make sense?

We also further apply a Jacobian matrix to the X and Y covariance values (sorry, not familiar with the markup:

Jac = [cos(yaw) -sin(yaw) sin(yaw) cos(yaw)]
measurementNoiseCov = Jac * measurementNoiseCov * Jac.t()

Does this also make sense? I've tried reading up various papers, but I haven't really been able to make much sense of them.

Answer 1

The term "Kalman Filter" tends to be used in genetically and there are a number of variations suitable for different applications.

The documentation in OpenCV for the Kalman Filter describes a strictly Linear type which they say can be modified to be an Extended Kalman Filter (EKF). Your Jacobian implies that your problem is nonlinear, and the OpenCV version is exclusively a linearization. Many problems are solvable using this approach, but it works better, the smaller your time step (in other terms updated more often). You can also inflate your state covariance.

Another approach, where the state transition $\bf{x_k} \; \Rightarrow \;\bf{x_{k+1}}$ is nonlinear, and using an integration technique, typically Euler Integration is used on the State Evolution equation. The nonlinear state covariance update uses the Jacobian linearization. It doesn't look like you can use the OpenCV KF, for this kind of EKF.

One issue with the KF in general is choosing the number of states you use in your filter. A very common problem in applications is when there is a large change in state, like turns, the Filter exhibits a lot of lag. If an additional state was present to account for the change (acceleration in Kinematic models) the lag would be much lower, but the the higher dimension can make the Filter more sensitive to small disturbances. In some cases, this isn't a problem and in others there is the Interacting Multiple Model (IMM) or as a friend of mine called it "Dueling Kalman Filters". The OpenCV doesn't look like it provides the innovations, which probably means that IMM is not a candidate with an all linearized EKF.

The essential problem when you have a jump and don't account for the possibility of a jump, is that the Filter places more trust on the predicted state and the change looks like noise. It wants (as an anthropomorphic analogy) to keep the state level and steady. In many cases, the error accumulates slow enough for the Filter to adjust to the jump. In other cases, track is lost. State Covariance inflation is one way to have the Filter weigh the new measurements over the predictions based on the old state.

One way to avoid linearization problems and approximate integration is to used the Unscented KF, which is often a good solution. The state dimension problem can creep in with this filter a well.

The sensitivity to jump lag manifests itself differently to different forms of the KF and update rate dependency is present in all forms. Kalman Filters need a lot of tuning and testing in any case.

Step size or update rate implicitly effects time integration error. Small steps, less error. Nonlinear integration, less error.